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APPENDIX A 


NESS USER'S MANUAL 


This is a reference manual for NESS, a simulation expert-system 
developed at Vanderbilt University. This manual will give user information 
regarding starting and operating NESS. 

1. STARTUP 

To start NESS one must log onto the VAX 11/780 at Vanderbilt. The VMS 
prompt '$' appears on the screen. The user then executes the following 
commands: 

$ ness 

$ shell [This places you under "EUNICE" a UNIX-like operating 

system.] 

% lisp 1 % is EUNICE'S prompt. LISP gets you into the FRANZLISP 

environment.] 

Franz Lisp, Opus 38.79 
Do you want to run: 

1) NASA EXPERT Simulation SYSTEM ? 

2) GENIE (GENeric Inference Engine) ? 

3) Franz Lisp ? 

Please enter choice(s): 

# 1 

Before running the NASA Expert Simulation System, 
did you type in 'SHELL' while you were under VMS? 

1) yes 

2) no 

3) don't know 

Please enter choice(s): 

# 1 


Welcome to NASA Expert Simulation System (NESS) 


Loading GENIE, and NESS.... 

GENIE version 2.3 generated on Tues. Apr. 2 12:25:06 1985 
NESS version 1.0 generated on Thu. Jun. 27 11:45:05 1985 
[be patient this may take one or two minutes] 


This expert system provides an intelligent interface to a generic 
simulation program for spacecraft attitude control problems. Below is a 
menu of the functions the system can perform. Control will repeatedly 
return to this menu after executing each user request. When you are ready 
for further text display, hit the 'return' key: 
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2. NESS OPERATION 


Now you are in the control of NESS. Its TOP LEVEL MENU will appear on 
the screen. Please make only one choice at a time. The menu appears as 
follows: 

top_level choice 
top_l eveljnenu 

1) Exit to GENIE 

2) Set up initial parameter values 

3) Run simulation program 

4) Display current parameter values required for simulation 

5) Display outputs generated by simulation 

6) Change initial parameter values required for simulation 

7) Set up initial parameter values to default values 

8) Store current parameter values in a disk-file 


Please enter choice(s). Now you have to enter the number of the 
function you want to be performed. You are strongly advised to enter only 
one choice at a time. 

A brief explanation of what happens when each of the choices is 
selected follows. 

1) Exit to GENIE 

You can use this function to halt NESS and go to the GENIE environ- 
ment. To return to NESS use option 2 of GENIE' s menu. This is not advised 
for those not familiar with GENIE. 

2) Set up initial parameter values 

This function allows you to enter initial values of parameters neces- 
sary for the simulation experiment. NESS will ask you some questions to 
infer or to provide values. 

An option is provided here to allow users to run both types of 
responses with the same values for parameters. The first question asked by 
NESS concerns this aspect. 

NESS assumes the inertial matrix to be diagonal with all diagonal 
elements equal. Hence it prompts the user for only one diagonal element of 
the inertial matrix. 

NESS asks some questions to get values of the Kp and Kd controller 
matrices. If proportional control is not desired then all elements of the 
Kp matrix are set to 0.0. Likewise if differential control is not wanted 
then all elements of the Kd matrix are set to 0.0. If no cross-coupling is 
desired the Kp and Kd become diagonal matrices, otherwise, the user is 
asked whether he wants them to be symmetric or regular. If the user wants 
the same controller gain for all axes in a diagonal matrix, then NESS 
prompts the user for only one element of that particular matrix. Depending 
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on the reply from the user, NESS configures the Kp and Kd matrices. 

If the user wants the Quaternion block to be included in the simula- 
tion, then NESS prompts for initial values of the Quaternion. 

NESS asks the user to choose a system response type from a menu of 
responses (step response or frequency response). 

Some questions are asked about initializing angular velocity and 
angular position. They include whether the user wants all 0.0 initial 
values, the same initial values for all axes, or different initial values 
for the three axes. 

For other parameters NESS simply asks the user to enter values. NESS 
knows about the parameters required for each response type, and only asks 
for the values of the required parameters. 

3) Run simulation program 

After using option 2 to set initial values of parameters, the user 
can use this option to run the the simulation program. This function first 
calculates eigenvalues of the system matrix. From the eigenvalues it 
infers the value of Tfinal for step response (from some heuristics). It 
displays this value to the user, and, if the user wants to have a dif- 
ferent value for Tfinal, he can override this value. For the other two 
types of responses, NESS displays the value of TAU or the time constant of 
the system. The FORTRAN program gets this value, and, by using some 
heuristics, it calculates Tfinal, for each frequency separately. 

If either Kp or Kd or both contain all 0.0 elements, NESS warns the 
user about this. In this case, NESS goes to a default value of TAU 
(default =1.0). 

Finally the user has the option of not running the simulation 
program. If he wants to return to the TOP LEVEL MENU to review and alter 
some parameter values, he can do so. 

If the user wants to run the simulation program, the FORTRAN program 
is executed. After execution, control is returned to the TOP LEVEL MENU. 

If the user picks this option, (i.e., run simulation program before 
values have found for parameters) then NESS simply displays a message and 
returns to the user to set up initial parameter values. 

4) Display current parameter values required for simulation 

Using this option the user can look at parameter values present in 
the database of NESS. A menu of parameters known to NESS is displayed and 
the user can choose the parameters whose values are to be displayed. If 
the parameter has a value it is displayed, else "nil (empty frame level)" 
is displayed signifying that that particular parameter does not have any 
value. The user can return to the TOP LEVEL MENU using option 1 (Return to 
TOP level) present on this menu. 



If none of the parameters have any values then NESS displays a mes- 
sage to that effect. 

5) Display outputs generated by simulation 

This option allows the user to look at the results of the simulation 
experiment. A menu of outputs generated by the simulation is displayed and 
the user can choose the output that is to be displayed. The outputs gener- 
ated by the simulation program are: plot of omega, plot of theta, numeri- 
cal outputs and characteristics of the analysis. The user can return to 
the TOP LEVEL MENU by choosing option 1 on this menu. 

NESS will not display any outputs if the simulation program has not 
been run, or has not executed properly. It will display a message to that 
effect to the user when he selects this option. 

6) Change initial parameter values required for simulation 

This option allows the user to change parameter values. A menu of 
parameters known to NESS is displayed and the user can choose the 
parameters whose values are to be changed. NESS then asks the user for the 
new value of that parameter. The user can return to the TOP LEVEL MENU by 
choosing option 1 present on this menu. 

Let it be noted that the value entered by the user is stored in the 
database by NESS without checking its validity. Thus a non-numerical value 
can be entered for a parameter, leading to an error later on. This error 
will be detected by the FRANZLISP interpreter when it tries to create the 
data file of parameter values for the FORTRAN program. The user will have 
to do his own checking to find out the wrongly entered value. Hence this 
option must be used with caution. If an error occurs, it is best to use 
option 2 of the TOP LEVEL MENU to gather values for parameters. 

The user will not be allowed to change values of parameters, if none 
of them have any value. This message will be displayed to the user when he 
selects this option. 

7) Set up initial parameter values to default values 

The user can use values stored in a disk file to be be entered in 
NESS's database by selecting this option. NESS displays the current con- 
tents of the current subdirectory and asks the user to enter the file 
containing initial values. The file entered by the user must have been 
created by NESS using option 8 of the TOP LEVEL MENU. Any other file must 
not be entered. If this is done unexpected errors might occur. 

8) Store current parameter values in a disk file 

The user can store current parameter values in NESS's database to a 
disk file. The stored values can be used for later experimentation by using 
option 7 of the top level menu. NESS displays the current contents of the 
current subdirectory and asks the user to enter a filename, where the 
current initial values will be stored. The user must not enter a filename 
already existing in the current subdirectory unless he is sure that doing 



so will not endanger a file crucial to NESS. The files crucial to NESS are 
given below. 

simxpert.l, rules. 1, change. 1, user.l , demonar.l, 
lisprc.l, demonOO.l, demonOl.l, demon02.1, 
demon03.1, eigenval.o and main.exe. 



ENGLISH VERSION OF THE RULES IN VARIOUS RULE-BASES 


1) value_input_rb: 

Rule_l: IF [all simulation parameters are known] 

THEN [run the simulation] 

Rule_2: IF [the inertial matrix is full] 

and [the Kp matrix is full] 
and [the Kd matrix is full] 
and [the Quaternion is initialized] 
and [an integration method is found] 
and [the type of response is known] 
and [the axis of input command is known] 
and [the value of TO is known] 
and [the value of error is known] 
and [the Y matrix is full] 

THEN [the first_step parameters are known] 

Rule_3: IF [step_response was chosen] 

and [first_step_parameters are known] 
and [the value of deltaT is known] 
and [the value of steady state error is known] 
or [frequency_response was chosen] 
and [first_step_parameters are known] 
and [second_step_parameters are known] 

THEN [all parameters are known] 

Rule_4: IF [the value of amplitude is known] 

and [the value of initial_freguency is known] 
and [the number_of_samples per_period is known] 
and [the value of number_oT_decades is known] 
and [the value of number_of_sampling_frequency_per_decade 
is known] 

and [the value of phase is known] 

THEN [the second_step parameters are known] 

Rule_5: IF [derivative control is not desired] 

THEN [Kd_matrix_type is zero] 
and [Kd matrix is full] 
and [eaBi element of Kd_matrix=0] 

Rule_6: IF [proportional control is not desired] 

THEN [Kp_matrix_type is zero] 
and [Kp matrix is full] 
and [eaBi element of Kp_matrix=0] 

[cross-coupling_between_axes for proportional control is 
not desired] 
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Rule 7: IF 



THEN 

and 

Rule 8: IF 


THEN 

and 

Rule_9: IF 

and 

and 

THEN 

and 

Rule_10: IF 

and 

and 

THEN 

and 

Rule_ll: IF 

and 

and 

and 

and 

THEN 

Rule_12: IF 

and 

and 

and 

and 

THEN 

Rule 13: IF 


THEN 

and 

and 

and 

Rule_14: IF 

and 

and 

and 


[Kp_matrix_type is diagonal] 

[each off-diagonal element of Kp_matrix=0] 

[cross -coupling_between_axes for differential control is 
not desired] 

[Kdjnatrixjtype is diagonal] 

[each off-diagonal element of Kp_matrix=0] 

[cross-coupling_between_axes for proportional control is 
not desired] 

[the same proportional control is desired for all axes] 

[the value of Kp(l,l) is known (user will be asked)] 

[Kpjnatrixjtype is equal diagonal terms] 

[each diagonal element oT Kp_matrTx=Kp(l,l)] 

[cross -coupling_between_axes for differential control is 
not desired] 

[the same differential control is desired for all axes] 

[the value of Kd(l,l) is known (user will be asked) 

[Kdjnatrix type is equal_di agonal terms] 

[each diagonal element of KdjnatrTx=Kd(l,l)] 

[cross-coupling_between_axes for proportional control is 
not desired] 

[the same proportional control is not desired for all axes] 
[the value of Kp(l,l) is known (user will be asked)! 

[the value of Kp(2,2) is known (user will be asked)] 

[the value of Kp(3,3) is known (user will be asked). 

[Kpjnatrixjtype is not equaljdiagonaljterms] 

[cross -coupling_between_axes for differential control is 
not desired] 

[the same differential control is not desired for all axes] 
[the value of Kd(l,l) is known (user will be asked)! 

[the value of Kd(2,2) is known (user will be asked)! 

[the value of Kd(3,3) is known (user will be asked). 

[Kdjnatrixjtype is not equaljiiagonaljterms] 

[the value of inertialjnatrix (1 1) is known (user will be 
asked)] 

[inertialjnatrix (2 2) * inertialjnatrix (1 1)] 
[inertialjnatrix (3 3) = inertialjnatrix (1 l)] 
[inertialjnatrix is full] 

[each off-diagonal element of inertialjnatrix = 0] 

[cross-coupling_between_axes for proportional control 
is desired] 

[the Kp matrix is symmetric] 

[the value of Kpjnatrix(l.l) is known (user will be asked)] 
[the value of Kpjnatrix(l,2) is known (user will be asked)] 
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and [the value of Kp_matrix(l,3) is known (user will be asked)] 

and [the value of Kpjnatrix(2,2) is known (user will be asked)] 

and [the value of Kpjnatrix (2, 3) is known (user will be asked)] 

and [the value of Kpjnatrix (3, 3) is known (user will be asked)] 

THEN [Kp matrix type is symmetric] 
and [KpT2,l)=Kp(l,2)] 
and [ Kp (3,1) =Kp (1,3)] 
and [Kp(3,2)=Kp(2,3)] 


Rule_15: IF [cross-coupling_between_axes for differential control is 

desired] 

and [the Kd matri; 


and [the value of 
and [the value of 
and [the value of 
and [the value of 
and [the value of 
and [the value of 


x is symmetric] 

Kdjnatrix (1,1) is known (user will be asked) 
Kdjnatrix (1,2) is known (user will be asked) 
Kdjnatrix (1,3) is known (user will be asked) 
Kd_matrix(2,2) is known (user will be asked) 
Kdjnatrix (2, 3) is known (user will be asked) 
Kd matrix(3,3) is known (user will be asked)! 


THEN [Kd matrix type is symmetric] 
and [KdlE.l^lO’U.E)] 
and [Kd(3,l)=Kd(l,3) ] 
and [Kd(3,2)=Kd(2,3)] 


Rule 16: IF 


and 

and 

and 

and 

and 

and 

and 

and 

and 

and 

THEN 


[cross -coupling_between_axes for proportional 
desired] 

[the Kpjnatrix is not symmetric] 

[the value of Kpjnatrix (1,1) is known (user wi 

[the value of Kpjnatrix (1,2) is known (user wi 

[the value of Kpjnatrix (1,3) is known (user wi 

[the value of Kpjnatrix (2,1) is known (user wi 

[the value of Kpjnatrix(2,2) is known (user wi 

[the value of Kpjnatrix (2, 3) is known (user wi 

[the value of Kpjnatrix (3,1) is known (user wi 

[the value of Kp matrix (3,2) is known (user wi 

"t,3) i 


control is 


[the value of Kpjnatrix (3, 


[Kpjnatrixjtype is regular] 


s known (user wi 


be asked)! 
be asked V 
be asked) 
be asked) 
be asked) 
be asked) 
be asked) 
be asked)! 
be asked). 


Rule 17: IF 


and 

and 

and 

and 

and 

and 

and 

and 

and 

and 

and 

and 


[cross-coup! ing_between_axes for differential 
is desired] 

[the Kdjnatrix is not symmetric] 

[the value of Kdjnatrix (1,1) is known (user wi 

[the value of Kdjnatrix (1,2) is known (user wi 

[the value of Kdjnatrix (1,3) is known (user wi 

[the value of Kdjnatrix(2,l) is known (user wi 

[the value of Kdjnatrix (2, 2) is known (user wi 

[the value of Kdjnatrix(2,3) is known (user wi 

[the value of Kdjnatrix(2,3) is known (user wi 

[the value of Kdjnatrix (2, 3) is known (user wi 

[the value of Kd matrix(3,l) is known (user wi 

Lthe value of Kdjnatrix (3, 2) is known (user wi 

’the value of Kd matrix(3,3) is known (user wi 


control 


11 be 
11 be 
11 be 
11 be 
11 be 
11 be 
11 be 
11 be 
11 be 
11 be 
11 be 


asked) 

asked) 

asked) 

asked) 

asked) 

asked) 

asked) 

asked) 

asked) 

asked)! 

asked)! 


[Kdjnatrix_type is regular] 
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Rule_18: IF [the Kp_matrix type is diagonal] 

and [the Kpjnatrix~has equal diagonal terms] 
or [the Kpjnatri xjtype is diagonal] 

and [the Kpjnatri x has unequal diagonal terms] 
or [the Kp_matrix_type is symmetric] 
or [the Kpjnatri xjtype is regular] 

THEN [Kp_matrix is full] 

Rule_19: IF [the Kdjnatri xjtype is diagonal] 

and [the Kdjnatri x has equal diagonal terms] 
or [the Kdjnatri x_type is diagonal] 

and [the Kdjnatri x has unequal diagonal terms] 
or [the Kdjnatri xjtype is symmetric] 
or [the Kdjnatri xjtype is regular] 

THEN [Kdjnatri x is full] 

Rule_20: IF [the Quaternion is wanted by the user] 

and [the value of roll angle is known] 
and [the value of pitcF_angle is known] 
and [the value of yawjangle is known] 

THEN [the Quaternion is initialized] 
and [initial value of theta for all axes to be 0.0 is wanted] 

ELSE [the Quaternion is initialized] 

Rule_21: IF [initial value of theta for all axes to be 0.0 is wanted] 

THEN [the value of tcontrol 1 is true] 
and [theta jc = theta_j = tFeta_z =0.0] 

Rule_22: IF [initial value of theta for all axes to be equal is wanted] 

and [the value of theta_x is known] 

THEN [the value of tcontrol_2 is true] 

and [theta_y = theta_x] 

and [theta_z = theta_x] 

Rule_23: IF [initial values of theta for all axes to be unequal is wanted] 

and [the value of theta_x is known] 

and [the value of theta_y is known] 

and [the value of theta_z is known] 

THEN [the value of tcontrol_3 is true] 

Rule_24: IF [the value of tcontrol_l is found to be true] 

or [the value of tcontrol_2 is found to be true] 

or [the value of tcontrol_3 is found to be true] 

and [the value of ocontrol_l is found to be true] 

or [the value of ocontrol_2 is found to be true] 

or [the value of ocontrol_3 is found to be true] 

THEN [the Y matrix is full] 
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Rule_25: IF [initial value of omega for all axes to be 0.0 is wanted] 

THEN [the value of ocontrol_l is true] 
and [omega_x = omega_y =omega_z = 0.0] 

Rule_26: IF [initial values of omega for all axes to be equal is wanted] 

and [the value of omega_x is known] 

THEN [the value of ocontrol_2 is true] 
and [omega_y = omega_x] 
and [omega_z = omega_x] 

Rule_27: IF [initial values of omega for all axes to be unequal is wanted] 

and [the value of omega_x is known] 
and [the value of omega_y is known] 
and [the value of omegajz is known] 

THEN [the value of ocontrol_3 is true] 

Rule_29: IF [integration method selected is Euler] 

THEN [the value of error is 0.0] 

2) run_rb: 

Rule_l: IF [response type chosen is step response] 

and [the value of Tfinal has been found] 
or [response type chosen is frequency response] 
and [the user wants to run the simulation program] 

THEN [run the simulation program] 

Rule_2: IF [response type chosen is step response] 

and [user likes the calculated value of Tfinal] 

THEN [the value of Tfinal has been found] 

ELSE [ask user to provide his value for Tfinal] 
and [the value of Tfinal has been found] 

3 ) output_di spl ay_rb : 

All rules in this rule_base are of the same form viz 

IF [user wants to observe a particular output from 
the simulation program] 

THEN [display that output on the screen] 

4) disp_init_val_rb: 

All rules in' this rule_base are of the same form viz 

IF [user wants to observe the value of a particular parameter] 

THEN [display the value of that particular parameter on the screen] 
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5) change_param_rb: 

All rules in this rule_base are of the same form viz 

IF [user wants to change the value of a particular parameter] 

THEN [ask the user to provide his value for that particular 
parameter] 

and [set that particular parameters value to the value given 
by the user] 


6) change_matrix_rb: 


Rule 1: IF 


[user wants to change the value of an element of a matrix] 


THEN 

and 


7) stage_l_rb: 

Rule_l: IF 

and 

THEN 

Rule_2: IF 

and 

THEN 

Rule_3: IF 

and 

THEN 

Rule_4: IF 

and 

THEN 

Rule_5: IF 

and 

THEN 

Rule_6: IF 

THEN 

Rule 7: IF 


[ask user for row column and value] 

[set the element in the given row and column to the 
given value] 


[user wants to observe outputs generated by the simulation 
program] 

[simulation run has been successful] 

[display a menu of the various outputs generated by the 
simulation] 

[user wants to run the simulation program] 

[all simulation parameter values are known] 

[run the simulation program] 

[user wants to observe values of parameters] 

[all simulation parameter values are known] 

[display a menu of the various parameters known to NESS] 

[user wants to store parameter values in the database 
of NESS to a file] 

[all simulation parameter values are known] 

[store parameter values to file whose name is supplied 
by user] 

[user wants to run the simulation program] 

[all simulation parameter values are not known] 

[display message saying user cannot run simulation program] 

[user wants to set parameter values to default values] 

[set parameter values to values found in file whose name 
is supplied by user] 

[user wants to change values of parameters] 
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Rule_8: 
Rule 9: 


Rule 10: 


Rule 11: 


Rule_12: 
Rule 13: 


8) user 
Rule 1: 


and [all simulation parameter values are known] 

THEN [display a menu of the parameters known to NESS] 

IF [user wants to set up parameter initial values] 
and [all simulation parameter values are known] 

THEN [run the user_rb] 

IF [user wants to observe the outputs generated by the 
simulation program] 

and [the simulation run has not been successful] 

THEN [display a message telling the user that outputs cannot 
be displayed] 

IF [user wants to change values of parameters] 
and [all simulation parameter values are not known] 

THEN [display a message telling the user that parameter values 
cannot be changed] 

IF [user wants to observe values of parameters] 
and [all simulation parameter values are not known] 

THEN [display a message telling the user that he cannot observe 
the values of parameters] 

IF [user wants to set up parameter initial values] 
and [all simulation parameter values are not known] 

THEN [run the value_input_rb] 

IF [user wants to store parameter values in the database of 
NESS to a file] 

and [all simulation parameter values are not known] 

THEN [display a message telling the user that parameter values 
in the database of NESS cannot be stored in a file] 

rb: 

IF [user wants to change current response type] 

THEN [all current parameter values will be saved and user 
will be prompted for a new response type] 

ELSE [all current parameter values except the response type 
will be deleted from the database of NESS] 
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APPENDIX B 


NESS PROGRAMMER'S MANUAL 


FRAMES USED IN NESS 

There are two types of frames used in NESS, static frames and dynamic 
frames. Static frames contain time invariant knowledge while dynamic 
frames created either by rule-bases or by the menu-input stage contain 
dynamic knowledge. 

Static Frames 


A list of all static frames used in NESS is available in the frame 
named tt file_index" and slot named "frames." This frame is present in the 
file simxpert.l . Static frames store rule-bases, menu input control 
knowledge, agendas, system specs, param specs and static daTa. Contents of 
each static frame can be found in the computer listing of NESS given in 
Appendix D. A brief description of the static data frames used in NESS 
follows. The three data frames used in NESS are: top_l evel_menu , 

outputjdi spl ayjnenu and param_menu. 

top_level_«enu 

This frame contains the top level menu of NESS i.e., the eight main 
functions of NESS. This frame looks as follows. 

(top level menu 

" (Exit to GENIE) 

(Set up initial parameter values) 

(Run simulation program) 

(Display current parameter values required for simulation) 


(Store current parameter values in a disk-file)) 

This frame is used by the menu_input frame top_level_control . 
output_di spl ay_»enu 

This frame contains the menu of the outputs that can be generated by 
FORTRAN simulation program. This frame looks as follows. 

(output display menu 

"(Return to TOP LEVEL MENU) 

(Plot of omega) 

(Plot of theta) 

(Numerical output generated by simulation)) 

This frame is used by the menu_input frame outputjdi spl ay_control . 
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param_aenu 

This frame contains a menu of all parameters known to NESS. It con- 
tains eighteen parameter names like TO, Tfinal, Kp matrix etc. This frame 
looks as follows. 

(paramjnenu 

(Return to TOP LEVEL MENU) 

(Response type) 

(TO) 

(Tfinal) 


(Steady state error)) 

This frame is used by the menu_input frames disp_init_val_control and 
change_param_control . 

Dynami c Frames 

As mentioned earlier, dynamic frames store data obtained during an 
interactive session with NESS. The main purpose of these frames in NESS is 
to store values of parameters required by the simulation software. The 
frames used to store parameter values are initial value, Kp, Kd, 
inertialjnatrix. Quaternion, Y matrix and response_cHbsen. Attributes 
about the parameters are store? In the frames Kpjnatrix, Kdjnatrix, 
inertial mat and controller typejdesired. A listing of these frames 
created cTuring a session with""NESS can be found in Figure B.l. All the 
above mentioned frames are created by the val ue_i nput_rb rule-base. 

initial_value 

This frame stores values of parameters which are scalar in nature. It 
also stores some attributes concerning these parameters and some control 
knowledge required for an ordered firing of the rules in the backward- 
chained rule-base value_input_1nput_rb. The parameter values are stored in 
slots having the name of their respective parameters. The value of TO is 
stored in the slot TO, the value of deltaT is stored in the slot deltaT 
and so on. Attributes about parameters are stored in slots like 
frame_full, quaternion_initialized, init_theta=0_wanted, etc. Control 
knowledge for ordered firing of rules are stored in slots like 
first_step_found, second_step found, tcontrol_l, ocontrol_l, etc. This 
frame is used by the disp_iniT_val_rb, run_rb, change_param_rb and user_rb 
rule-bases. 

Kp Kd and inertialnatrix 

These three frames are similar to each other, each containing the 
values of the controller or inertial matrix it is named after. Since all 
the three matrices contain nine elements, each of these frames contain 
nine slots, each slot corresponding to a unique element in the matrix. For 
example slot (1 2) corresponds to the element in the first row and second 
column of the matrix. These frames are used by the disp_init_val_rb and 
change param rb rule-bases. 


B.2 



(Kp ( (1 2) (0. 0 ) > 

( ( 1 3) (0. 0) ) 

< <2 l > to on 

<(2 3) (0. 0) > 

((31) (0. 0) > 

((3 2) (0. 0) ) 

((1 1 ) ( 1 ) > 

< (2 2 ) ( 1 ) ) 

( <3 3) (1 > ) ) 

<Kd < <1 2) (0. 0) ) 

( ( 1 3) (0. 0) ) 

( (2 1 ) ( 0 . 0 ) ) 

( (2 3) (0. 0) ) 

1(3 1) (0. O) ) 

((3 2) (0. 0>> 

((1 1) (1>> 

((2 2) (1>> 

((3 3) (1>>> 

(inertial_matrix ((1 1) <l>) 

(( 33 ) ( 1 ) ) 

((2 2 ) ( 1 )) 

((1 2) (0.0)) 

((1 3) <0.0>> 

((2 1) <0.0>> 

< (2 3) (0. 0) > 

((31) (0. 0) ) 

( (3 2) (0. 0) ) > 

(Kp_matr 1 x (matr i x_type (diagonal)) 

(equal_diagonal_terros (trua)) 

(matr i x_f ul 1 (true))) 

(Kd_matrix (matr i x_type (diagonal)) 

(equal_diagonal_terms (true)) 

(natrix_full (true))) 

( inert! al_mat (matr i x_f ul 1 (true))) 

(control ler_type_desired (proportional (yes)) 

<prop_cross_coup_bet_axes (no)) 
(same_proportional_control_for_all_axes (yes) ) 
(derivative (yes)) 

(diff_cross_coup_bet_axes (no)) 
<same_differential_control_for_ail_axes (yes))) 
(respons e_chosen (response_type ((Frequency response)))) 

(quaternion (uanted_by_user (no))) 

( initial_value ( qua tern i on_in i tial i zed (true)) 

( integration_method (Euler)) 

(a»is_of input command (X>) 

(TO (0>) 

(error (0. 0) > 

( init_theta=0_wanted (yes)) 

(tcontrol_l (true)) 

( init_omega«0_«iianted (yes)) 

(ocontrol_l (true)) 

(V_r*trix_full (true)) 

(first step_found (true)) 

(deltaT (0.01)) 

(steady state error (2>) 

< f ra-.e_f ul 1 (true)) 

(FfinaT (10. 0>) 

(anp 1 itude (1 > > 

( ini t_f requency_value (0. 0159154) ) 

(nunber_of_samp les_per_per iod (256) ) 

(nu*ber_of_decades (3)) 

(nunber_of_sampling_frequency_per_decade (3) ) 

(phase (0>> 

<second_step_found (true))) 

(Y matrix (theta > (0.0) > 

(theta_y (0. 0) > 

(theta_z (0.0) > 

(omega_> (0. 0) > 

(omega_y (0. 0) > 

(omega_i (0. 0) ) ) 

i • An example of dynamic frames created by NESS. 
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Kpjnatrix Kdjiatrix and inertialjwtt 

These frames contain attributes about the controller or inertial 
matrix they are named after. This is done by using slots like matrix_type, 
matrix_full, equal_diagonal_terms, etc. 

control 1 er_type_desi red 

This frame contains attributes about the type of controller desired 
by the user. This is done by using slots like proportional, derivative, 
etc. 


response_chosen Quaternion Y_»atrix 

All these three frames store initial values. The first stores the 
type of response chosen by the user, the second stores the initial values 
of the Quaternion and the third stores initial values of the angular 
position and the angular velocity. These frames are used by the 
disp_init_val_rb and change_param_rb rule-bases. 

In addition to these frames, four other frames are also created to 
store certain choices indicated by the user. They are top_level_choice, 
output_display_choice, disp_init_val_choice and change_param_choice. 

top_l evel_choi ce 

This frame stores the choice made by the user from the TOP LEVEL MENU 
of NESS. It is created by the top level_agenda frame and is used by the 
stage_l_rb rule-base. It looks as foTlows. 

(top_level_choice 

(top_level_menu 

(Run simulation program))) 


output_di spl ay_cho1 ce 

This frame stores the choice made by the user from menu of outputs 
generated by the simulation program. It is created by the 

output di spl ay_agenda frame and is used by the output_di spl ay_rb rule- 
base. Tt looks as follows. 

( output_di spl ay_choi ce 

(outputjdisplay menu 

(Plot oT omega))) 


di sp_ini t_val _choi ce 

This frame stores the choice made by the user from the menu of 
parameters known to NESS, for the purpose of displaying the value of the 
chosen parameter. It is created by the disp init_val agenda frame and is 
used by the disp_init_val_rb rule-base. It looks as foTlows. 
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{ di sp_i ni t_val_choi ce 
(param menu 
“ (TO))) 


change_param_choi ce 

This frame stores the choice made by the user from the menu of 
parameters known to NESS, for the purpose of changing the value of the 
chosen parameter. It is created by the change param agenda frame and is 
used by the change_param_rb rule-base. It looFs as Tollows. 

(change_param_choice 
(param menu 

” (deltaT) )) 


Another frame called "user" is used in NESS to describe certain 
events and to store some attributes. This frame is created by the run_rb 
rule-base and is used by the stage_l_rb rule-base. It looks as follows. 

(user 

(simulation_run 

(error_free)) 

(likes Tfinal value 
(yes)T) 


DEMONS USED IN NESS 

Demons are special purpose FRANZ LISP functions written to perform 
specific tasks incapable of being performed by GENIE. Demons are also used 
to perform tasks for which GENIE techniques might prove to be inefficient 
from the point of execution time, programming time and memory use. A brief 
description of the demons used in NESS follows. 

Demons Present in File DEM0N00 .1 

start_sim 

This demon is used to run the FORTRAN-based simulation program in the 
FRANZ LISP environment. 

store_val ues_in_l i sparray_from_frame 

This demon is used to store values in a FRANZ LISP array from a frame 
created by NESS. 

printarra^y 

This demon is used to display the values of an array or a matrix on 
the screen, with each row of the array displayed on one line on the 
screen. 
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1 oad_ei gen_val ues_i n_f rame 

This demon calls a fortran function to calculate the eigenvalues of 
the system matrix. It then stores these eigenvalues in a frame. An heuris- 
tic about calculating Tfinal is also incorporated in this demon. 

f requency_output_di spl ay 

This demon is used to display some of the outputs generated by the 
simulation program in case a frequency response is simulated. 

clear_display 

This demon is used to clear the screen, 
del im_di splay 

This demon is used to print certain characters on the screen. 

Demons Present in File DEM0NQ1 .1 
setup_1 ni t_val_i n_simul a . i np 

This demon is used to store in a file the parameter values in the 
format required by the simulation program. The simulation program gets its 
input data from this particular file. 

Demons Present in File DEM0N02 .1 

conf i rn 

This demon is used by the matrix_change_rb rule-base to acquire 
information from the user. 

change 

This demon is used by the matrix_change_rb rule-base to change the 
value of an element of a matrix. 

change_quaterni on 

This demon is used to change the initial values of the Quaternion. 
Demons Present in File DEMQNQ3 .1 
startJM terate 

This demon is used to keep NESS in operation at its TOP LEVEL MENU 
until the user decides to exit from NESS by using option "Exit to GENIE" 
provided on the TOP LEVEL MENU. 

loop 

This demon is used to keep NESS at one of its secondary menu stages 
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i.e., the output_di spl ay_menu used to select a simulation output for 
display or the paramjnenu used to either display or change the values of 
parameters. The user can go to the TOP LEVEL MENU by using the option 
"Return to TOP LEVEL MENU" present on these secondary menus. 

storage_of_i ni ti al_yal ues 

This demon is used to perform function 8 of the TOP LEVEL MENU i.e., 
"store current parameter values in a disk file." 

change_Yjna tr i x 

This demon is used to change the values of initial angular velocity 
and the initial angular position. 

Demons Present in File DEMON AR .l 

ar r a y_a cces s_from_f raise 

This demon takes as arguments a name of an array or a matrix, a row 
number and a column number and returns a path to a frame containing ele- 
ments of that particular array, down to the slot for that particular row 
number and column number. 

store_yal ues_i n_frame_from_l i spvector 

This demon is used to store values from a FRANZ LISP vector to a 
frame created by NESS. 

Demon Present in File LISPRC .l 

ness 

If you have exited to GENIE level after a session with NESS, running 
this demon from FRANZ LISP will enable you to come under the control of 
NESS without having to go back to the operating system level. 

Coupling GENIE and the Simulation Software Using VAX 11/780 and FRANZ LISP 

NESS is an expert system which utilizes the GENIE system as Its 
inference engine. GENIE is documented elsewhere [GENIE Ref Manual] and is 
written in FRANZ LISP which is a LISP dialect designed for the UNIX 
operating system environment. FRANZ LISP provides a number of ways that a 
LISP process such as NESS can Implement operating system calls. These 
calls allow NESS to do things like write a disk file containing the 
parameters that the simulation program needs, cause its execution and read 
the output files It creates as illustrated in Figure . 

The most straightforward utilization of a system call is simply to 
include a FRANZ LISP function "exec" [Franz Manual CH 4] to cause the 
execution of a standard UNIX command, directly in a rule clause. For 
example outputjdi spl ay rb_rule6 checks the precondition that Insures that 
the simulation program Was run and that the user wants to see a plot of 
theta which the simulation program would have deposited in a disk file 
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called "thetaOplt.stp." The 'then' side of the rule looks as follows: 

($then (exec cat thetaOplt.stp)). 

This causes the UNIX "cat" command to execute which simply copies the 
named file (thetaOplt.stp) to the user's terminal. 

A slightly more involved method is to write a demon (i.e., a special 
purpose LISP function) to perform some specific operation which may in- 
volve one or more calls to UNIX system functions. For example, a demon 
named "setup ini t val in_simula.inp” calls the system functions 

"fileopen," "cTose, - "' ancT "cprintf" [see Franz Manual] which does formatted 
file write operations. This demon is called by run_rb_rul el . 

This rule also calls the demon "start sim" which uses the FRANZ LISP 
function "process" [Franz Manual CH 6] to fork a child process which is 
the actual execution of the simulation program. The code for the rules and 
the demons is listed in Appendix D. 
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APPENDIX C 

THEORETICAL BACKGROUND ON QUATERNION 


1. INTRODUCTION 


This appendix reviews the concepts and properties related to 
Quaternion. The definition and the algebra of Quaternion are covered in 
the following sections. 


2. DEFINITION 

In his classical book entitled Elements of Quaternions, Sir W. R. 
Hamilton defined Quaternion as 

"In fact it will be shown .... that there is an important 
sense in which we can conceive a scalar to be added to a 
vector; and that the sum so obtained, or the combination, 

'Scalar plus Vector,' is a Quaternion." (Hamilton) 

Throughout his book other definitions related to Quaternions can be found, 
where geometrical relations in the cartesian plane and space are involved. 
For example, Hamilton pointed out that 

"this essential connection of the complex relation between 
two lines, and which we have given the name of a geometrical 
quotient, with a system of four numerical elements, we have 
a motive for saying that the quotient of two vectors is 
generally a Quaternion." 


Thus, we can expect from such a concept a variety of possible defini- 
tions for various applications. For example, a most widely used definition 
found In the aerospace literature states that 

"a Quaternion is a four-parameter system for uniquely 
specifying the attitude of a rigid body with respect to some 
reference frame." (Grubin) 


The applicability of Quaternions to the solution of digital attitude 
control problems which involve transformations among different coordinate 
systems offers some advantages over the equivalent direction cosine solu- 
tion (Ickes). As an example, let us consider the Space Telescope Pointing 
Control System (Glaese, et al.). The initial design was changed in 1974 to 
reduce program cost. They include: 1) moving the annular Support Systems 
Module components nearer to the composite center of mass; 2) elimination 
of the Image Motion Compensation system; 3) employment of four reaction 
wheels instead of single-gimbal control moment gyroscopes; and 4) perfor- 
mance of the emergency and backup functions by a magnetic torquer system. 
Significant cost reduction was obtained through the selection of reaction 
wheels as primary controllers and magnetic torquers as auxiliary actuators 
for momentum management and backup to the primary controllers in case of 
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failures. For overall system simplicity, strapdown integration is done in 
terms of the Quaternion, i.e. relative four parameter attitude variables. 
The advantages for using the Quaternion were summarized as: 

1. a single algorithm for all attitude control modes; 

2. small vehicle attitude errors at all times; 

3. continuous updating of the strapdown calculation through a drift rate 
correction added to the reference frame rate; and 

4. easy shaping of maneuver, scan or other rate profiles. 

In the following, basic algebraic manipulations of Quaternions are 
described. 


3. ALGEBRA OF QUATERNIONS 

An extension of the previous definitions on the Quaternion may state 
that the Quaternion is a generalization of the set of complex numbers for 
the study of rotational motion (Glaese and Kennel). The mathematical 
expression for a Quaternion Q is given by 

Q = EQ X Q 2 Q 3 Q 4 ] = <Q Q 4 ) = Q 4 + iQ x + jQ 2 + kQ 3 (C-l) 

where Q represents the vector part or imaginary part of the Quaternion, Q4 
the real or scalar component, and 

i = j = /^I, k= /IT (C-2) 


3.1 SUM OF QUATERNIONS 

Let Q and R be Quaternions. Then, the sum of Q and R, called S, is 
given by: 

S = Q + R = (Q Q 4 ) + (R R 4 ) = (S S 4 ) (C-3) 

where, 

S = Q + R = i (Q 1 +R 1 ) + j(Q 2 +R 2 ) + k(Q 3 +R 3 ) (C-4) 

and 

s 4 ■ Q 4 +R 4 (c - 5) 


Therefore, the sum of Quaternions is commutative and associative. 


3.2 PRODUCT OF QUATERNIONS 

Because a vector and a real number are related to a Quaternion, the 
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following conditions are satisfied (Hamilton): 

i*i = j*j = k*k = -1 


(C-6a) 


and 


ixj=k, jxk = i,kxi=j (C-6b) 

The product of two Quaternions is then defined as follows: 

P Q o R = (Q Q 4 ) o (R R 4 ) = (P P 4 ) (C-7) 

where 

P = Q^ + R^ + Q x R (C-8) 

and 

p 4 " R 4 - £ • i ( C- 9) 

In the above equations, {.) and (x) represent the dot and cross product, 
respectively. 

From equations (C-8) and (C-9) we observe that the (o) product is 
associative and distributive, but it is not commutative. 


3.3 CONJUGATE OF QUATERNIONS 

The conjugate of a Quaternion Q is given by: 

Q = (1Q X + JQ 2 + kQ 3 + Q 4 )* = Q 4 -iQ x - jQ 2 - kQ 3 (C-10) 


3.4 INVERSE OF A QUATERNION 

By combining definitions (C-7) and (C-10) we can define the inverse 
of a Quaternion. First let us consider 

Q o Q* = Q* o Q = Ql 2 +Q2 2 +Q3 2 + Q4 2 (C-ll) 

then, the inverse of the Quaternion Q, Q^“^, is given by: 

1 1 

Q, '[ ] . Q* (C-12) 

- 1 (Q* o Q) Q* o Q 


3.5 TRIPLE PRODUCT OF QUATERNIONS 

In order for us to study rotational motions in the three-dimensional 
space, we have to define the triple product of Quaternions, V. The triple 
product is given by: 


V » Q* o V o Q 


(C-13) 
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This expression does not mix scalar and vector parts. For the vector part 
note from equation (C-13) that 

(r )* = (Q* o V o Q)* = Q* o V* o Q = -Q* o V o Q * -V' (C-14) 

3.6 LENGTH OF VECTORS 

From the previous results, we can find the length of a vector as 
f ol 1 ows : 


|v '| 2 = Y*./ - ± o (vV = 

= (Q* o V O Q) 0 (o* 0 V 0 Q)* = 

■ (Q* o V o Q) o (Q* o V* o Q) = (C-15) 

= Q* 0 V O (q O Q*) 0 V* o Q = 

. * ,2 * 

= (q oqnov 

It can be concluded that the length of a vector is multiplied by the 
factor (Q* o Q) , which is a real number. Thus, from equation (C-15) we 
notice that the length of a given vector _V' in the prime coordinate frame 
is related to the norm of the vector by 

|v' j = (Q o Q*) 2 |VJ 2 (C-16) 


4. VECTORIAL ALGEBRA OF QUATERNIONS 

This section provides a closer look at the vectorial algebra of 
Quaternions. 


4.1 NORMALIZATION 

The real number (Q* o Q) is defined as the “common norm" (Hamilton). 
It can be assumed without loss of generality that 

Q* o Q = 1 (C-17) 

and the concept of normalized Quaternions is introduced. Equation (C-l) is 
equivalent to the following expression: 

Q = cos(0/2) + sin(0/2) . £ ( C-18) 

where real and imaginary parts of the Quaternion are distinguished, and 
the vector £ is an unitary vector (i.e., u. . _u = 1 ). Equation (C-18) 
preserves the vector length. 
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4.2 ROTATION OPERATOR 


From previous results, the rotation of a vector around another vector 
with some angle may be found using Quaternions. Substituting (C-18) into 
(C-13), we get 

V* = CO$(0) . V + SIN(0) . (u X V) + [l-COS(0)](_u . Y)u (C-19) 

which is the general expression of a rotation of a vector around the 
axis of the unitary vector £, with an angle of rotation 0. 

Equation (C-19) is the same expression as the general rotation trans- 
f ormati on 


Rot(k, <f>) 


‘ k x k x \eTs<fr + cos^ 
k x k y \eTs<f> + k t sin ^ 
k x k t \ers4> - k y sin 4> 
0 


k v k x ven4> — k t sin ^ 
kyky\ers<t> + cos^ 
ky k x xers<j> + k x sin <f> 
0 


k x k x \en4> + sin 4> O' 
k M kyvenji — k x sin 4> 0 
k t k x \en<f> + cob# 0 
0 1 . 


(C-20) 


The vector V is obtained by multiplying the matrix Rot(Jc,0) with V_. 
For details, see (Paul) or (Glaese & Kennel). 


4.3 TIME-RATE CHANGE OF A VECTOR 

The rate of change of the norm with respect to time is obtained by 
taking derivatives of equation (C-17): 

d/dt(Q* o Q) = (Q)* o Q + Q* o Q = 0 (C-21) 

From this equation it is shown that 

Q* o Q = - Q* o Q * - (Q* o Q)* ( C-22 ) 

which represents a particular vector. Let 

Q* o Q = 1/2 w (C-23) 

Premultiplying this equation by Q, we obtain 

Q * 1/2 Q o w 


The evaluation of the rate of change of a vector with respect to time 
in two reference frames is performed in the following way: 

d/dt(V) = d/dt(Q o V* o Q*) = 
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(C-25) 


= Q o V 


★ 

o Q + 


Q o d/dt( V 


o Q*) + 


Q o V o Q 


★ 

Substituting d/dt(Q) and d/dt( Q ) obtained from equation (C-24) into 
equation (C-25), the rate of change of the vector is 

d/dt(V) = Q o [d/dt(v‘ ) + w x v‘] o Q* (C-26) 

In this equation w is identified as the relative angular velocity of the 
primed axes with respect to the unprimed (Glaese & Kennel). 

The differential equation represented in equation (C-24) is known as 
the "Quaternion-rate equation" (Grubin). In the next section, a more 
detailed treatment of this equation and its implications are presented. 


5. MATRIX ALGEBRA OF QUATERNIONS 

The matrix representation of a Quaternion turns out to be a more 
convenient way of representing various Quaternion operations. For example, 
equation (C-7) defined the Quaternion product as 


P 


Q o R = ( 


Q^R + R^ + Q x R ^ 
Q4R4 - Q • R 


(C-27) 


which can also be written as 


P = Q 0 R = 


+q 4 

-o 3 

+Q- 

( 

+ q 3 

+q 4 

-Q 

-<>2 

+Q 1 

+ Q, 


-q 2 

-Q; 



Q R 


(C-28) 


This representation clearly shows that the matrix Q satisfy Quater- 
nion properties and comprises "a matrix representation of Quaternion 
algebra with matrix multiplication corresponding to ( 0 )" (Glaese & 
Kennel). The product in equation (C-28) can be also expressed in the 
alternate form: 


P = 


+R 4 

+R 3 

- R 2 

+R 1 

- R 3 

+R 4 

+R 1 

+R 2 

+R 2 

' R 1 

+R 4 

+R 3 

- R 1 

- R 2 

- R 3 

+R 4 


Ql 

^2 

<>4 


- R Q 


(C-29) 
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Therefore we can show that: 


- Q 

R 

= 

R 

Q 

(C-30) 

= A 

0 

B 

0 

C = (A o B ) o C = (A~o~B) C = A B C = 



_ 



_ __ 

(C-31) 

= C 

B 

A 

= 

A C B = C A B 


o C 

S 

C 

B 


(C-32) 


Hence A C = C A, which is a very useful result. 


5.1 QUATERNION-RATE EQUATIONS 

In Section 4.3 the rate change of a vector V and the Quaternion-rate 
equations were derived. In this section, by applying some of the Quater- 
nion matrix operations to them, more convenient representations will be 
derived. 

First, let us consider equation (C-19) in product form: 

V' = Q* o V o Q = Q* V Q = Q* Q V (C-33) 

I 

Vectors^ and in the three-dimensional space are related through 

v' ® H V (C-34) 

where M is a direction cosine matrix and is a 3x3 matrix which transforms 
coordinates to the primed coordinates _V' by means of Q, i.e., 

M = Q* Q (C-35) 

Referring now to equation (C-19) and substituting 0 by -0, we have 

H = u_ - sin(0)u + cos(0)[I - u. u. T ] (C-36) 

The matrix u is called the "cross product matrix" (Glaese & Kennel) and is 
formed by dropping the final row and final column of u. The identity 
matrix is represented by I and its dimension depends on the current im- 
plementation. 

Solving equation (C-35), the expanded form of the direction cosine 
matrix is given in terms of the Quaternion parameters by (Glaese & 
Kennel ): 

Q 1 2 - Q 2 Z " Q 3 2+Q 4 2 2( Q 1 Q 2 +Q 3 Q 4 1 2( Q 1 Q 3" Q 2 Q 4 } 

M = 2( Q 2 Q 1 -Q 3 Q 4 ) -Q 1 2+ Q 2 2 -Q 3 2 +Q 4 2 2( Q 2 Q 3 + Q 1 Q 4 ) (C— 37) 

2( Q 3 Q 1 + Q 2 Q 4 ) 2( Q 3 Q 2 -Q 1 Q 4 ) -Qi 2 -Q 2 2+q 3 2+q 4 2 
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Finally, the Quaternion-rate equations are expressed in matrix form as 


Q = 1/2 Q o w = 1/2 w Q = 1/2 


or 


Q = 1/2 Q o w = 1/2 Q w = 1/2 


0 

+w 3 

-w 2 

” w 3 

0 

+W 1 

+W 2 

" W 1 

0 

" W 1 

-w 2 

- w 3 

+ ^4 


+ q 2 

+ q 3 


-Ql 

-q 2 

+Q 1 

+ q 4 

-Ql 

”^2 

*3 


+w. 


w. 


w„ 


Wo 


*1 

*2 

q 3 

^4 


(C-38) 


(C-39) 
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NESS LISTING 



(changa_Darani_rb (i#_r(jmovad (d_remova_rula* ) ) 

(craat#d_by <»amir>> 

<creatad_on (I2_23_83l)> 

(rule* (changa_param_rb_rulal) 

<changa_param_rb_rula4> 

< chang a_param_rb_rul#9) 

( chang •_param_rb_rulalO) 

( chang #_param_rb_rol*ll) 

<changa_parafl>_rb_rulal2) 

< chang a_paran*_rb_rulal3) 

<change_param_rb_rulal7) 

< chang a_param_rb_rula 10) 

( chang a__param_rb_rula3> 

<changa_param_rb_rulaS) 

<chang*_param_rb_rul»14) 

<changa_param_rb_rulel3> 

( c hang »_param_rb_ru 1*16) 

(chang#_parain_rb_rul#7> 

(changa_param_rb_rula6> 

( chang#_param_rb_rula8) 

<changa_param_rb_rulal9) 

<change_param_rb_rule2) ) 

<param»_in_if t 
(changa_param_cholca 
<param_menu 

(rula <changa_param_rb_rulel ) 

<change_param_rb_rula4> 

<change_param_rb_rula9) 

< changa_param_rb_rulalO) 

<change_param_rb_rulall ) 

< changa_param_rb_rulal2) 

(changt_param_rb_rulal3) 

(changa_param_rb_rulal7) 

<changa_param_rb_riilalB) 

(change_param_rb_rula3> 

<c hang a_param_rb_ru 1*3) 

(changa_param_rb_rulal4) 

(changt_param_rb_rul«13> 

( chang •_param_rb_m 1*16) 

<changa_param_rb_rula7) 

(changa_param_rb_rula6> 

( changa_param_rb_.ru la8) 

(changa_param_rb_rulal9) 

<changa_param_rb_rule2> > ) ) ) > 
(changa_param_rb_rulel (*type (ifall)) 

<*if (trip la (changa_param_cholca param_manu) - TO)) 
(craatad_by (aamlr)) 

<cr#at#d_on (I2_27_B3I)> 

(•than (frem '( ini t ial_valua TO)) 

(dallm_dl*play > 

<clau«a_prlnt > 

(clao*a_print 

lEntar nau valua of atartlng tlma (TO):l) 
<m*g " » "> 

(fput ' ( ini t ial_valua TO) (raad))) 

(modiflad by ( I Juan J. Rodrlguai fc Bar-Jau Halahl)) 
(modif iad_on ( I6_14_83l > > > 

(changa_param_rb rula4 (*typa (ifall)) 

<*if 

(tripla ( changa_param_choica param_manu) 



(Multi-step integration errorl)) 

(created_.bg (samir)) 

<craatad_on (I2_27_83l)> 

(•than (frem ' ( ini t ia l_va lua error)) 

(delim_dlsplag) 

(clau«a_pr int) 

(clausa_pr lnt 

(Enter nau valua of multi-step integration arror: I > 

(msg " » ") 

(fput ' ( ini t ial_valua arror) (raad))) 

(modif ied_bg (I Juan J. Rodriguez t< Bor-Jau Hiieht)) 

(mod if iad_on ( I6_14JB3I ) > ) 

(change_param_rb_rule9 (*tgpa (ifall)) 

(•if 

(triple (change_param_cholce param_manu) 

m 

(Amplitude of input wave eignalt)) 

(creatad_bg (samir)) 

(creatad_on (I2_27_83l>) 

(•then (fram ' ( ini t ial_valua amplitude)) 

(dal im_d iep lag ) 

(clau*a_print) 

(clau*a_print 

tEntar nau valua of amplitude of the input wave signal: I ) 
(mag " » "> 

(fput ' ( init ial_valua amplitude) (read))) 

(modifiad_bg (IJuan J. Rodriguez St Bor-Jau Haiahl)) 

(modif iad_on ( I6_14_83l ) ) ) 

(change_param_rb rulalO (•tgpa (ifall)) 

(•if 

(triple (changa_param_cholca paramjnanu) 

m 

I Initial lowest freguencg I ) ) 

(creatad_bg (samir)) 

(craatad_on (I2J27JB3D) 

(•than (fram 

'( ini tial_valua init_froquoncg_valuo) ) 

(delim_displag ) 

(clause_print ) 

(clause_prlnt 

(Enter new valua of Initial lowest frequencg: I ) 

(msg " » ") 

(fput ' ( init ial_valua init_frequancg_yalua ) 

(raad ) > ) 

(modif ied_bg (IJuan J. Rodriguez & Bor-Jau Hsiehl)) 

(modif lad_on ( I6_14_83l ) ) ) 

(change_param_rb_rulell (Stgpa (ifall)) " 

(•if 

(triple (change_param_choice param_menu) 

■ 

INumbar of decades!)) 

(created bg (samir)) 

(created”on (I2_27_B3I>) 

(•then (frem ' ( ini t lal_value number _of_decades> ) 

( del im_d isp lag ) 

(clause_pr int ) 

(clause_print 

(Enter new value of number of decades:!) 

(msg " » ") 

(fput ' ( ini tial_value number_of _decades ) 

(read ) ) ) 



- (modif ied_bg (I Juan J. Rodriguai & Bor-Jau Haiehl)) 

(modi f i*d_on (16 14 831))) 

(ehange_param_rb_rulel2 <*tgpe (ifall)) ” 

(•If 

(triple (changa_param_choice param_manu) 

m 

[Number of eampling f requenc iee/decade I ) ) 

(created_bg (aamir)) 

(craatad_on (I2_27_B3I)) 

(•than (fram ~ 

* ( Ini t ial_valua 

numbar_of _*amp 1 ing_frequencg_per .decade) > 

(del lm_d iep lag ) 

(clauaa_print> 

(clausa_print 

lEntar nau valua of number of eampling frequenciea par dacada; I ) 
(m«g " » ") 

(fput ' ( init ial_valua 

numbar_of _aamp 1 ing_f requencg_per .decade } 

(raad) ) ) 

(modif lad_bg (I Juan J. Rodriguai & Bor-Jau Haiehl)) 

(mod if iad_on ( 16.14.831 >> > 

(changa_param_rb_rulal3 (*tgpa (ifall)) “ 

(•if 

(triple (changa_param_choice paramjnenu) 

IPhaaa of input wave eignall)) 

(craatad_bg (aamir) > 

(created_on (I2_27_83D) 

(•than (fram ' ( initial_valua phaae)) 

(dalim_diaplag ) 

(clauae_print > 

(clauaa_pr int 

lEntar new valua of phaaa of tha input wave aignal:l> 

(mag " » "> 

(fput ' ( initial_valua phaaa) (read))) 

(modif ied_bg (I Juan J. Rodriguai ti Bor-Jau Heieh!)) 

(modif iad_on (16.14.831))) 

(changa_param_rb_rulel7 (*tgpe (ifall)) 

(•if 

(triple (change_param_cholce param_m»nu) 

(Amplitude of impulael)) 

(craated_bg (aamir)) 

(created_on (I2_27_83H) 

(•then (fram ' ( initial.value impulee_amplitude> ) 

(delim_diaplag ) 

(clauaa_print) 

(c lauaa_pr int 

lEntar new valua of amplitude of impulae: I > 

(mag " » «) 

(fput '(initial_value impulaa_amplituda) 

(read ) ) ) 

(modified bg (IJuan J. Rodriguai 1c Bor-Jau Heieh!)) 

(modif ied_on ( I6_14_83l ) ) ) 

(changa_param_rb_rulel8 (*tgpe (ifall)) 

(•if 

(triple (change_param_choice param.menu) 
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IRaaponaa tgpa I ) ) 
(created_.bg (aamir)) 





- (created_on (I2_27_85l>) 

(•then ( dc 1 im_d itp leg > 

( c laute_pr int ) 

(clauae_print 

I Th* ratponte type mutt not ba changad from thia laval. bacauta ofl) 
(clau«a_pr int 

Itha lack of coniittancy chacking. Plaata uaa option 2 of tha TOPI) 
(clauta_pr int 

(LEVEL MENU to changa 'Ratponta type'. I>) 

(modifiad_by (IJuan J. Rodriguez & Bor-Jau Htlahl)) 

(mod if iad_on (I6_1489l)>> 

( chang #_param_rb_rula3 (ttype (ifallM ~ 

(•if 

(tripla (changa_param_choica param_menu ) 

(DaltaT (tima increment) I ) ) 

(created_by (tamir)) 

(created_on (I2_27_83l>> 

(•than (fram '( ini t ial_valua deltaT)) 

(delim_ditplay ) 

(clauae_print > 

(clauea_print 

lEntar nau valua of tima incramant (daltaT):t) 

(mtg " » -> 

(fput ' ( initial_value deltaT) (read))) 

(modified_by (IJuan J. Rodriguez t< Bor-Jau Htlahl)) 

(mod if ied_on ( I6_14_83l > > > 

(change_param_rb_rule3 (*type ( ifall >> 

(•if 

(tripla (changa_param_choica paramjnenu) 

(Steady etate error I )) 

(creatad_by (tamir)) 

(craatad_on (I2_27JB3I)) 

(•than (fram '( ini tial_valua ateady_etate_arror > ) 

(dalim_dltplay ) 

(clauta _pr int) 

(clauta_pr int 

lEntar new valua of tteady atata error: I ) 

(mtg ” » ") 

(fput ' ( ini t ial_valua tteady_ttate_arror > 

(read ) ) ) 

(modifiad_by (IJuan J. Rodriguaz ti Bor-Jau Htlahl)) 

(modif ied_on ( I6_14_B3I > > > 

(change_param rb rulal4 (ttype (ifall)) 

(•if 

(tripla (change_param_cholce param_pienu) ■ IKp Matrix l)> 

(creatad_by (tamir)) 

(created_on (I2_27_83l>) 

(•than (dellm_dieplay ) 

(fput '(matrlx_change matrix) 'Kp ) 

(fput '(matrix_changa datlred) 

'yet > 

(reiet_rula_baea 'change_matr ix_rb ) 

(forward 'change_matr i x_rb ) ) 

(modifled_by (IJuan J. Rodriguaz t< Bor-Jau Htlahl)) 

(modif ied_on ( I6_14_8SI > > > 

(change_param_rb_rulal3 (ttype (ifall)) 

(•if 

(triple (change_param_cholce param_menu> “ IKd Matrix!)) 

(craatad_by (tamir)) 
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(craatad.on (12.27.831)) 

(•than ( da 1 im_d i »p lag ) 

(fput '(matrix.changa matrix) 'Kd) 

(fput ' (ir.atrix.chang* daslrad) 

'gaa ) 

(raaat.rula.bax* 'changa.matr 1 x.rb ) 

(forward 'changa.matrix.rb ) ) 

(modlfiad.bg (I Juan J. Rodriguax l< Bor-Jau Haiahl)) 

(mod if lad_on ( I6_14_Q3I > ) ) 

(changa_param_rb_rulal6 (*tgpa (ifall)) ” 

(•if 

(trlpla (chang*_param_choica paramjtanu) 

m 

llnartlal Matriil)) 

(craatad_bg (aamlr)) 

(cr#atad_on (12.27.831)) 

<*th»n < dal im_d i mp lag ) 

(fput ' (matrli_changa matrix) 

' inart ial_matr lx ) 

(fput '(matrix.changa daairad) 

'gaa ) 

(raaat.rula.bax* 'changa_matrl x_rb ) 

(forward 'changa.matrix.rb ) ) 

(modlf iad_bg (I Juan J. Rodriguax ti Bor-Jau Haiahl)) 
(modlf iad_on < 16.14.831 ) ) ) 

(chang e_param_rb.ru l a7 («tgpe (Ifall)) 

(•if 

(trlpla (changa_param_choica param_manu) ■ Qua tarn Ion)) 
(craatad.bg (aamlr)) 

(craatad.on (I2J27.83U) 

(•than (dallm.dlaplag) (change.quaternion) ) 

(modlf iad.bg (IJuan J. Rodriguax ti Bor-Jau Haiahl)) 
(modlf iad.on (16.14.831))) 

(chang* jar am_rb.ru 1 at («tgpa (ifall)) 

(•if 

(trlpla (changa_param_cholca paraijaanu) 

(Initial valuaa of Omaga and Thatal)) 
(craatad.bg (aamlr)) 

(craatad.on (12.27.831)) 

(•than (dalim.diaplag) (changa.Y.matrix ) ) 

(modiflad.bg (IJuan J. Rodriguax ti Bor-Jau Haiahl >> 
(modlf Iad.on (16.14.831))) 



( change_param_rb_.ru le8 (*tgpe (ifall)) 

(created_bg (tamir)) 

(creat»d_on (12 27JBSI)) 

(•if 

(tripla (change_param_choice param_menu ) 

m 

IFrequencg deltaTD) 

(•than (frem 

' ( initlal_valua frequencg_de!taT) ) 
(dellm_displag) 

<clau«e_print ) 

(clause_print Enter 
new 
value 
of 

Frequencg 
del taT. ) 

(clau«e_print 


(Isg*^ *° Uo * in * v * lu «« 256 or S12 or 10241) 

<fput ' ( initial_value 

f requeue g_dei taT) 

(read) ) ) 

(mod If ied_bg (aamlr)) 

(modif led_on ( IB_1 BSD)) 
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(chango_param_rb_rulel9 (*type (ifall) > 

(•if 

(triple (change_param_choice param_menu ) 

lAxis of input command))) 

(creatod_by (ISamiri Andy# and Juanl)) 

(creatod_on (I6_13_83l)) 

(•than (from 

'( initlal_value axls_of_lnput_command ) ) 

(dolim_display) ” 

(clause_print) 

(clause _print 

lEntor tho now valuo for 'Axis of input command 'I ) 

(clausejirint 

ITho only valuot pooolblo aro X Y Z or nonol) 

( c lau»»_pr int 

lEntor X Y or Z (uppor caso only) or 'none' (lower caioll) 

(m*y « » ") 

(fput ' ( initial_valuo axls_of_lnput_command) 

(road > ) > 

(modified by (oamir)) 

(modif iod_on ( I6_24_83l > ) ) 

( change_param_rb_rule2 (*typo (ifall)) 

(•if (triple (change_param_choico param_monu) - Tfinal)) 

(croatod_by (oamir)) 

(croatod_on (I2JZ7JB9D) 

(•then (c lause_print 

(You can change the value of Tfinal when you run the simulation program)) 
(clauoo jirlnt 

luting option 3 of the TOP LEVEL HENUI ) 

(claut e_prlnt 

INo change in value of Tfinal it poteible now))) 

(modlfied_by (oamir)) 

(modif led_on ( I 6_24_8S I ) > ) 

(change_matrix_rb (lf_removed (d_remove_rulet) > 

(created_by (oamir)) 

(created_on (I2_27_83l)) 

(rules <change_matri«_rb_rulel ) ) 

(paramo_in_ifo ~ “ 

(matrix_chango (desired (rule (chango_matrii_rfc_rulel ) > > ) ) ) 

( change_matr i x_rb_rulel (Stype (ifall)) ~ 

(•if (triple (matrlx_change desired) ■ yet) (confirm)) 

(•then (change) 

(reset_rulo_baso 'change_matr ix_rb ) ) 

(•else (from 'matrix_chango> ) 

(created_by (semir)) 

(croated.on ( I 2_27 JBS I ) ) ) 



(daf atart_aim - * 

(lambda nil 

(prog (rat_coda) 

(aatq r#t_coda (aprocaaa 'main)) 

(cond ((naq rat_coda 0) 

(tarpri) 

(nag N "♦ — - — — — — — +» ) 

(mag N "I Simulation tarminatad abnormally. I H > 

(nag N — - — — _________ — + n j 

(raturn nil)) 

(t (fput '(uaar aimulation_run) *arror_fraa) 

(fput '(uaar laat_raaponaa_run) 

(car 

(fgat_valuaa 

' (raaponaa_choaan raaponaa_typa ) ) ))>>))) 

(daf a tor a_va luaa_in_l iaparray_f rom_f rama 
(lambda ( array _nama ) ~ ~ 

(prog (1) 

(aatq i 0) 
loopl(prog (j) 

(aatq j 0) 

loopSCatora (array_nama i j) 

(timaa 1.0 
(car 

(f gat_valuaa 

(array_accaaa_from_frama array_nama 

(+ i l> 

(+ j 1>>)>>> 

(aatq j (+ j 1)) 

(cond ((aq j 3) (raturn nil))) 

(go loop2>) 

(aatq i <+ 1 1>) 

(cond ((aq 1 3) (raturn nil))) 

(go loopl)))) 

(daf printarray 

(lambda (array_nama) 

(prog (1) 

(aatq 1 1) 
loopl(prog (j) 

(aatq j 1) 

loop2(cpr intf "%# " 

(timaa 1.0 
(car 

(fgat_valuaa 

(array_accaaa_from_frama array_nama 1 j))))) 

(aatq j <♦ j 1)) 

(cond ((aq j 4) (raturn nil))) 

(go loop2>> 

(tarpri ) 

(aatq i (♦ 1 1>> 

(cond ((aq i 4) (raturn nil))) 

(go lcopl)))) 

(daf load_aigan_valuaa_in_f rama 
(lambda nil 

(prog (rat_codol) 

(aatq rat_codal 

(aigan (gatd 'Kp> 

( gatd 'Kd) 

(gatd ' inart ial_matri « ) 

(gatd 'ona_ovar_tau> 

(gatd 'aigr> 
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- (ge%d ' * i g i ) > ) 

(cond ((equal ret_codel O.O) 

(cond 

((equal ( one_over_tau 0) 0.0) 

(nog N "Uncontrolled system. Mill not roach steady otata. ") 
(nog M 

"Probablo cauio: Kd or Kp matrix hao all 0.0 elements. " ) 
(nog N "Tfinal hao boon oot to dofault value **> 

(store (one_over_tau 0) 1.0))) 
(store_values_in_frame_from_lispvactor 'one_over_tau 1) 
(otore_valueo_in_frame_#rom_l iopvector 'eigr 6) ~ 
(otore_valueo_in_frame_froin_liopvector 'eigi 6) 

(nog N "Eigenvalues computation has been succesful. ") 

(terpri) 

(cond 

((nog (car 

(fget.valuoo 

' (reoponoe_chooen responoe_type) ) ) 

'IStep reoponoel) 

(print (quotient 1.0 (one_over_tau 0))) 

(terpri ) 

(nog N “Given above is the calculated value of TAU. “ ) 

(terpri) ) ) ) 

(t (nog N “Eigenvalues calculation has not been succesful. ”> 

(nog N “Probable cause: the Inertial Matrix is singular.”) 
(msg N "Tfinal has been set to default value. “) 

(store (one_ovor_tau 0) 1.0)))))) 

(def froquency_output_display 
(lambda (x) 

(prog (a) 

(msg N "Do you want to display for ” ) 

(print x) 

(msg N ” 1) Amplitude. ”> 

(msg N " 2) Phase. "> 

loopKmog N "Please enter choice:”) 

(msg N " # ") 

(setq a (read) ) 

(cond ( (eq a 1 ) 

(cond ((eq x 'theta) 

(exec cat thetaOamp. frq) 

• (wait_a_uhile) 

( c lear_d isp lay ) ) 

(t (exec cat omegaOamp. frq) 

(walt_a_whlle) 

(clear_disp lay ) ) ) ) 

((eq a 2) 

(cond ((eq x 'theta) 

(exec cat thetaOpha. frq) 

(wait_a_whlle) 

( c 1 ear_d isp lay ) ) 

(t (exec cat omegaOpha. frq) 

(wait_a_while) 

( c lear_d lop lay ) ) ) ) 

(t (nog N “Your entry is not recognixed. ” ) (go loopl)))))) 

(def clear_display 
(lambda nil 

(princ (ascii 27)) 

(princ (ascii 91)) 

(princ (ascii 72>) 

(princ (ascii 27)) 

(princ (ascii 91)) 
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ii Modified on Jun-15-85 (j«jr) 

(def setup_init_val_ln_simula. inp 
(lambda nil 

(prog (xl x2 x3 x4 <S *6 x7 >8 x9 xlO xll xl2 xl3 xl4 xlS r q m dbin) 
(ntq dbin (fileopen "Simula, inp" " mi ")) 

(••tq r (car (fget_values ' (rasponsa_chosan rasponie_type) ) ) > 
<iatq q (car (fget_valuas '(quaternion uianted_by_user ) ) ) ) 

(satq m “ 

(car (fgat_valuas ' ( ini tial_valua integration method)))) 
(letq xl 'T> 

(satq x2 'F> 

(cond ((aq r 'IStap rasponsal) 

(cprintf "Xs " xl dbin) 

(cprlntf "Xs " x2 dbin) 

(cprintf "Xl " x2 dbin)) 

((aq r 'IFraquancy rasponsal) 

(cprintf "Xa * x2 dbin) 

(cprintf "Xs “ xl dbin) 

(cprintf "Xl " x2 dbin)) 

(t (cprintf "Xi " x2 dbin) 

(cprintf "Xi " x2 dbin) 

(cprintf "Xi " xl dbin))) 

(cond ((aq q 'yes) (cprintf "Xi “ xl dbin)) 

(t (cprintf "Xs " x2 dbin))) 

(cond ((aq m 'Euler) 

(cprintf "Xs " xl dbin) 

(cprintf "Xs " x2 dbin) 

(cprintf "Xi " x2 dbin)) 

((aq m ' IFourth-ordar Runga-Kutta I > 

(cprintf "Xi " x2 dbin) 

(cprintf "Xi " xl dbin) 

(cprintf "Xs " x2 dbin)) 

(t (cprintf "Xs " x2 dbin) 

(cprintf "Xs ” x2 dbin) 

(cprintf "Xs " xl dbin))) 

(tarpri dbin) 

(satq xl (times 1.0 (car (fget_values ' ( initial_value TO))))) 
(satq x4 (times 1.0 (car (fgat_valuei '( init ial_value error))))) 
(satq xS 0. 0) ~ 

(satq x6 0. 0) 

(satq x7 O.'O) 

(satq x8 0. 0) 

(satq x9 0. 0) 

(satq xlO 0. 0) 

(satq xll 0. 0) 

(satq xl2 0. O) 

(satq xl3 0. 0) 
t (satq xl4 0. 0) 

(satq xl3 0. 0) 

(cond ((aq r 'IStap rasponsal) 

• (satq x2 

(times 1.0 
(car 

• (fget_valuai ' ( ini t ial_valua Tfinal))))) 
(satq x3 

(times 1. 0 

• (car 

(fget_values ' ( initial_valua daltaT))))) 

(satq x6 

- (times 1. 0 

(car 






(fget_valua« 

' ( initial_value *teady_stcte_error ) ) ) ) ) ) 
((eq r 'llmpulse response!) 

(••tq x2 (quotient 1.0 (one_over_tau 0))) 

(satq *3 

( times 1. 0 
(car 

(fgat_valuaa ' ( ini t ial_value deltaT))))) 

(latq x7 

(tinea 1. 0 
(car 

(fget_values 

'( initial_value lmpulse_amplitude) 11)11 
(t ( aatq *2 (quotient 1.0 (one_over_tau 0))) 

(setq x3 

(time* 1. 0 
(car 

( f get_valuea 
'( ini tial_valu# 

frequency_deltaT) > I ) ) 

(aatq x8 

(times 1. O 
(car 

(fget_values 

' ( ini tial_value amplitude) >)> ) 

(setq x9 

(times 1.0 
(car 

(fget_valuea 

' ( ini t ial_value ini t_f requency value))))) 

(aatq xll 

(times 1.0 
(car 

(fget_valuas 

'(lnitial_value number _of_decades) ) > ) > 

(setq xlO 

(times 1.0 
(car 

(fget_values 

' ( lnitial_value phase))))) 

(setq x 12 

(times 1.0 
(car 

(fget_values 
' ( initial_value 


number_of_sampllng frequency_per_decade) )))))) 

(cprlntf H Xt * xl db in ) 

(cprintf "Xf " x2 dbin) 

(cprintf "Xf « x3 dbin) 

(cprintf "Xf " x4 dbin) 

(cprintf "Xf « xS dbin) 

(terpri dbin) 

(cprintf "Xf " x6 dbin) 

(cprintf "Xf " x7 dbin) 

(cprintf "Xf " xB dbin) 

(cprintf "Xf " x9 dbin) 

(cprintf "Xf " xlO dbin) 

(terpri dbin) 

(cprintf "Xf “ xll dbin) 

(cprintf "Xf " xl2 dbin) 

(cond ((eq 
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0 
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* 
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» 

> 
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! 

J 

3 

J 
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(car* 

<fgat_value« ' < ini tial_valua axia_of input command) >> 
'X> («et<i x 13 X. 0) ) 

( (aq 

(car 

( fgat_valuaa '(initial valua axla_of input command))) 
'Y) (aatq x 14 1.0)) 

( (aq 

(car 

(fgat_valua« '(initial valua axia_of input command))) 
'2) (aatq xl3 1. 0))) 

(cprintf -At " xl3 dbin) 

(cprintf -At M xl4 dbin) 

(cprintf —At “ x 13 dbin) 

(tarpri dbin) 

(*atq xl (tima» 1.0 (car (fgat_valua« '(Y_matrix omaga_x)))>) 

(tatq x2 (timaa 1.0 (car (Ygat_valuas '(Y_matrix omaga_y)>)>) 

(*atq x3 (timaa 1.0 (car (fgat_valuaa '(Y_matrix omaga_i ) ) ) ) ) 

(aatq *4 (timaa 1.0 (car (fgat_valuaa '(Y_matrix thata_x))))> 

(aatq x3 (timaa 1.0 (car (fgat_valuaa '(Y_matrix thata_y>>)>) 

(aatq x6 (timaa 1.0 (car (fgat_valuaa '(Y_matrix thata_x)))>) 

(aatq x7 0. 0) 

(aatq x8 0. O) 

(aatq x9 0. 0) 

(aatq xlO 0. 0) 

(cond 

( (aq q 'yaa) 

(aatq x7 

(timaa 1.0 

(car (fgat_valuaa '(quatarnion rol l_angle> ) ) ) > 

(aatq xB 

(timaa 1.0 

(car (fgat_valuaa '(quatarnion yau_angla> ) ) > ) 

(aatq x9 

(timaa 1.0 

(car (?gat_valuaa '(quatarnion pitch angla))))))) 

( cprintf "Xt - xl dbin) 

(cprintf -At - x2 dbin) 

(cprintf "Xt - x3 dbin) 

(cprintf M Xt “ x4 dbin) 

(cprintf -At ” x3 dbin) 

(tarpri dbin) 

(cprintf -At - tb dbin) 

(cprintf -At « x7 dbin) 

(cprintf "Xt - x8 dbin) 

(cprintf •'Xt ” x9 dbin) 

(cprintf " Xt - xlO dbin) 

(tarpri dbin) 

(prog nil 

(aatq x6 1) 

loopKcond ((aq tb 1) (aatq x9 'Kp ) ) 

((aq x6 2) (aatq x9 'Kd>) 

(t (aatq x9 ' inar t ial_matr 1 x ) ) ) 

(prog nil 

(aatq x7 1) 
loop2(prog nil 

(aatq x8 1) 
loop3(aatq xl 

(timaa 1.0 
(car 

(fgat_valuaa 


J 




(arrev accais from «9 

x7 

iB)))>) 

(cprintf "Xf H x 1 dbin) 

( *atq x8 ( + «8 1)) 

(cond ((aq, xB 4> (raturn nil))) 

(go Ioop3)> 

(aatq « 7 <+ x7 l>> 

(cond ((aq x7 4) (raturn nil))) 

(go loop2)> 

(tarpri dbin) 

(«atq x6 (+ <6 1>) 

(cond ((aq »6 4) (raturn nil))) 

(go loopl)) 

(cloaa dbin) 

(axae /atc/unixtovma ilmula. inp > ) ) ) 



I 


(daf confirm 
(lambda nil 
i (prog (an*) 

(tarpri > 

(fprint '(m*trix_change matrix)) 

I (msg N "Do gou want to altar ang alamants of tha abova matrix ?" N) 

(m*g N " 1) gas") 

(msg N " 2) no" N> 

loopKmsg N "Plaaia antar choica:") 

(msg N " ft ") 

(satq ans (raad) ) 

(cond ( (aq ans 1) (raturn '(1 (9 9)))) 

( (aq ans 2) (raturn '(-1 (9 9)))) 

(t (msg N "Your antrg is not racognlzad. Trg again. ") 

(go loopl)))))) 

(daf changa 
(lambda nil 

(prog (row col val mat) 

(satq mat (car (fgat_valuas ' (matrl x_changa matrix)))) 

(msg N "About tha alamant gou want to altar. Plaasoi") 

(msg N "- Entar tha row numbtr ■ ") 

(satq row (raad)) 

(msg N Entar tha col numbar ■ ") 

(satq col (raad)) 

(msg N Entar tha naw valua “ " ) 

(satq val (raad)) 

(fram (arrag_accass_from_frama mat row col)) 

(fput (arrag_accass_from_frama mat row col) val)))) 

(daf changa_quatarnlon 
(lambda nil 

(prog (abed) 

(msg N 

"Do gou want tha Quatarnlon block to be included in simulation 7" 

N> 

(msg N " 1) gas") 

(msg N " 2) no” N> 

loopKmsg N "Pleas* entar choice:") 

(msg N " * ") 

(satq a (raad) ) 

(cond ( (aq a 1 ) 

(prog nil 

r (fram '(quaternion wanted_bg_user ) ) 

(fput '(quaternion want*d_bg_usar ) 'gas) 

(msg N "Do gou want a naw valua for Roll angle ?") 

(msg N 

"Be sura gou had earlier entered a valua for Roll angle." 
N> 

> (msg N " 1) gas") 

(msg N " 2) no" N> 

loop2(msg N "Please antar choice:") 

> (msg N " # ") 

(satq b (read)) 

(cond ((aq b 1) 

(fram '(quaternion roll_angl*)> 

(msg N "Entar tha naw valua of initial angle: ") 

(fput '(quaternion roll.angla) (raad))) 

((aq b 2) (msg N "No changa to Roll angle.")) 

(t (msg N 

"Your antrg is not racogniiad. Trg again. ") 

(go loop2))> 

(msg N "Do gou want a new value for Pitch angle ?"> 


: 



• (meg N 

"Be tur* gou had eerier entered o value for Pitch angle. " 
• N> 

(mtg N " 1) get") 

(mag N " 2) no" N) 

i loop3(m»g N "Pleat* enter choice:") 

(mtg N “ ft ”) 

(tetq c (read)) 
i (cond ((eg c 1) 

(frem '(quaternion pitch_angl*> ) 

(mag N "Enter the new value of Initial angle: n ) 
i (fput '(quaternion pltch_angle) (read))) 

((eq c 2) (mtg N "No change to Pitch angle.")) 

(t (mtg N 

i "Your entrg it not recognixed. Trg again. M > 

(go loop3) ) ) 

(mtg N "Do gou want a new value for Yaw angle ?") 

(mtg N 

“Be ture gou had earier entered a value for Yaw angle. “ 

N) 

(mtg N " 1) get") 

(mtg N " 2) no" N) 

loop4(mtg N "Pleate enter choice:") 

(mtg N " ft ") 

(tetq d (read)) 

(cond ((eq d 1) 

i (from '(quaternion gaw_angle)) 

(mtg N "Enter the new value of initial angle: ") 

(fput '(quaternion gaw_angle) (read))) 

i ((eq d 2) (mtg N "No change to Yaw angle.")) 

(t (mtg N 

"Your entrg it not recognixed. Trg again.") 

r (go loap4))>)> 

( (eq a 2) 

(nag N 

i "The eimulatlon program will not ute the Quaternion block. ") 

(mtg N "That it. gou will bo running PROTOTYPE SYSTEM 0." N) 

(from '(quaternion wanted_bg_u*or > ) 

(fput '(quaternion wanted_bg_uaer ) 'no)) 

(t (nag N “Your entrg it not recognixed. Trg again. ") 

(go loopl)))))) 


I 



ii Modifiod by Juan J. Rotfr iguaz-Moacoa o & 
ii Bor-Jau Hslah (16-Jun-85) 

(da# atar t_&_itarata 
(lambda nil 
(prog nil 

loop (aval_aganda 'top_laval_aganda) 

(cond 

((eq (car (fgat_valuaa ' (top_lavel_choica top laval_pianu> ) ) 
' lExit to OENIEI ) 

(claar_diaplay > 

(raturn (gania)))) 

(tarpri) 

(tarpri ) 

(wait_f or_uaar ) 

(tarpri ) 

(tarpri ) 

(go loop ) ) ) ) 


(daf loop 

(lambda (option) 

(prog nil 

loopKcond ((aq option 'diap) 

(cond ((aq (car 

(fgat_valuaa 

'(diap_init_val_choica param nanu))) 

'IRaturn to TOP LEVEL MENU I ) 

(raturn nil)) 

(t (wait_for_uaar ) 

( c laar_d lap lay ) 

(aval_aganda 'dlap_lnit_val_aganda> ) ) ) 

((aq option 'changa) 

(cond ((aq (car 

(fgat_valuaa 

' (changa_param choica param manu) ) ) 

'IRaturn to TOP LEVEL MENU! ) 

(raturn nil)) 

(t (wait_for_uaar ) 

(claar_di*play ) 

(aval_aganda 'changa_param_aganda) ) ) ) 

((aq option 'out_diap) 

(cond ((aq (car 

(fgat_valuaa 

'(output_dlaplay choica output_dlapiay_manu> ) > 
'IRaturn to TOP LEVEL MENU I ) 

(raturn nil)) 

(t (wait_for_uaar ) 

(claar_di«play ) 

(aval_aganda 'output_d lap lay_aganda ) ) ) ) ) 

(go loopl)))) 


(da# atoraga_of_lnltlal_va 
(lambda nil 

( a t or a_f r ama a_i n_f i 1 a 


luaa 

'(Kp Kd 

inar t ial_matr 1 
Kp_matr i » 
Kd_matrlx 
inart 


a 


control lar_typa_daairad 

raaponaa_choaan 

quatarnion 

initial valua 
* 



" Y_matrix) 
(read ) ) > ) 


(def change_Y_natri> 

(lambda nil 
(prog (a) 

(meg N "Do you want to change the value of Theta(x) ?" N) 
(meg N " 1) gee") 

(meg N " 2) no" N) 

loopl(meg N "Pleaee enter choice:") 

(meg N " ft "> 

(eetq a (read) ) 

(cond ( (eq a 1) 

(frem '(Y_matri« theta_x>) 

(neg N "Enter the new value for Thata(i) ■ ") 

(fput '(Y_matrix theta_x> (read))) 

((eq a 2) (meg N "No change to Theta(x).")) 

(t (meg N "Your entry ie not recognlxed. Try again. ") 
(go loopl))) 

(meg N "Do you want to change the value of Theta(y) ?" N) 
(meg N " 1) yee") 

(meg N " 2) no" N) 

loop2(meg N "Pleaee enter choice:") 

(meg N " ft "> 

(eetq a (read) ) 

(cond ( (eq a 1 ) 

(frem '(Y_matrix theta_y)> 

(neg N "Enter the new value for Theta(y) ■ ") 

(fput '(Y_matrix theta_y> (read))) 

((eq a 2) (meg N "No change to Theta(y). ")) 

(t (neg N "Your entry ie not recogniied. Try again.") 
(go loop2>>) 

(meg N "Do you want to change the value of Theta(i) 7“ N) 
(meg N " 1) yee") 

(meg N " 2) no" N> 

loop3(meg N "Pleaee enter choice:") 

(meg N " ft ") 

(eetq a (read > ) 

(cond ((eq a 1) 

(frem '(Y_matrix theta_x>) 

(neg N "Enter the new value for Theta(x) “ ") 

(fput '(Y_matrix theta_x> (read))) 

((eq a 2) (meg N "No change to Theta(x).")) 

(t (meg N "Your entry ie not recognlxed. Try again. "> 
(go loop3>)) 

(meg N "Do you want to change the value of Omega(x) ?" N) 
(meg N " 1) yee") 

(meg N " 2) no" N> 

loop4(meg N "Pleaee enter choice:") 

(meg N " ft "> 

(eetq a (read ) ) 

(cond ((eq a 1 > 

(frem '(Y_matrix omega_x)> 

(neg N "Enter the new value for Omega(x) " ") 

(fput '(Y_matrix omega_x) (read))) 

((eq a 2) (meg N "No change to Omega(x).")) 

(t (meg N "Your entry ie not recognlxed. Try again.") 
(go loop4))> 

(meg N "Do you want to change the value of Omega(y) ?" N) 
(meg N " 1) yee") 

(meg N " 2) no” N) 
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Ioop5(mag N "Pleaae enter choice:”) 

(meg N " * ”> 

* (eetq. a (read ) ) 

(cond ( (eq. a 1 > 

(frem '(Y_matrix omega_y)) 

* (nag N "Enter the new value for Omega(y) “ ") 

(fput '(Y_matrix omega_y) (read))) 

<(eq a 2) (meg N “No change to Omega(y ). " ) ) 

* (t (nag N "Your entry la not recognitad. Try again.") 

(go loopS))) 

(nag N "Do you want to change the value of Omega(x) ?” N) 

* (nag N " 1) yaa") 

(mag N " 2) no" N) 

loop6(mag N "Plaaae enter choice:") 

•» (mag N " # ") 

( eetq a (read) ) 

(cond ((a< | a 1) 

* (frem '(Y_matri* omega_x)> 

(nag N "Enter the new value for Omaga(x) ■ ") 

(fput '(Y_matrix omega_x) (read))) 

* <(eq a 2) (mag N "No change to Omega(x).")) 

(t (mag N "Your entry ia not recognized. Try again. ") 
(go loop6>)))>) 





(daf array_accaaa_#rom_frame 

(lambda (array_nanta roui_num col_num) 

(Hit array_nama (llat row_num col_num>)>> 

(da# da#ina_arrag 

(lambda (arrag_nama rotu_bnd col_bnd> 

(array array_nama flonum row_bnd col_bnd>>) 

(da# «tora_valuaa_in_#rama_#rom_ll»pvaetor 
(lambda (array_nama roui_bnd> 

(prog (1) 

(aatq 1 1) 
loopKprog (j) 

(aat<| j 1) 

loop2(fran> (array_acca»a_from_frama arrag_nama 1 j>> 
<#put (array_acceaB_from_frame arrag_nama 1 j) 
(array_nama (- 1 1)>> 

(aatq j (+ j 1>) 

(cond ((an j 2) (raturn nil))) 

(go loop2)) 

(aatq i (+ i 1>) 

(cond ((an i (+ row_bnd 1)) (raturn nil))) 

(go loopl)))) 

(daf #orm_identity_matri * 

(lambda (array_nama row_bnd col_bnd) 

(prog (i) “ 

(aatq i 1) 
loopKprog (j) 

(•atq j 1) 

loop2(cond <(aq i j) (atora (arrag_nama i j) 1.0>) 

(t (stora (array_nama i j) 0. 0))> 

(aatq j ( + j 1>) 

(cond ((aq j (+ coi_bnd 1)) (raturn nil))) 

(go loop2)> 

(aatq i (+ i 1)) 

(cond ((aq i (+ row_bnd 1)) (raturn nil))) 

(go loopl)))) 
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ii Initialization program for running either: 

11 1> NASA Simulation Export System. 

ll 2) CENIE, or 

ii 3) Franz Lisp Opus 38.7? 

i i •* Written by 

ii Bor-Jau Hsioh (Andy) and 

ii Juan J. Rodr iguez-Moscoso (May-11-83) 

i l . Rovisod by 

ii Juan J. Rodr lguoz-Moscoto and 

ii Bor-Jau Hsioh (Andy) (Jun-0?-83) 

ii NEES moans NASA Export Simulation Systom required by usor in LISP 
1 1 (dof un noss ( ) 

ii (cond ((equal ans 1 ) (oval_agonda 'start_agenda) ) 

it (t (msg N "You should uso option 1 to start loading NESS" N ) 

ii (startup)))) 

(dofun noss( ) 

(start_&_iterate) ) 

it NESSJLOAD moans NASA Export Simulation System at "load" time. 

(dof ness_load 
(lambda nil 

(prog (ansuerl answer2) 

(clear_display > 

(head ing ) 

(torpri ) 

(msg N "Loading CENIE. and NESS (please bo patient)" N) 

(include /u/gait/henrik/genie) ) 

(set_up_frames_from_f ilo 'simxport. 1) 

(cloar_dtsplay ) 

(oval_agonda 'star t_agonda) 

(cloar_display 1 


n HEADING stands for tho heading of the NESS 
(dofun headlngO 

(msg N ) 
(msg N "I No looms to NASA Export Simulation Systom (NESS) I") 
(msg N ) 


3 

ii STARTUP stands for starting up tho Lisp environment 
(dofun startupO 

(prog (ansuor) 

(torpri) (torpri) 

(msg N "Do you want to run:") 

(torpri ) 

(nsg N " 1) NASA Export Simulation System ?") 

(nig N " 2) CENIE (GENoric Inference Engine) ?") 

(msg N " 3) Frani Lisp ?") 

(torpri) 

loop 

(nsg N "Please ontor cholco(s):") 

(msg N " # ") 

(sotq answer (read)) 

(cond ((equal answer 1) 

(clear_display ) 

(prog (ans) 

(msg N "Before running the NASA Expert Simulation System.") 
(msg N "did you typo in 'SHELL' while you wore under VMS?") 
( torpri ) 

(msg N " 1) yes") 

(msg N ” 2) no ") 



3) don't know") 


*(msg N •" 
loop 1 
(tarpri) 

(mag N "Please enter choice(a):") 

(m*g N “ « ") 

(aetq ana (read ) ) 

(cond ((actual ana lHraturn <naaa_load ) ) ) 

((actual ana 2) 

(mag N "You ara balng aant to tha Operating ") 
(princ “System you atartad In.") 

(mag N "Pleas# type 'SHELL' now if you ara ") 
(princ “under VMS 0/8.") 

(tarpri) 

(return (exit))) 

((actual ana 3) 

(mag N “You ara baing aant back to the "> 

(princ "Operating Syatam you ware before”) 

(mag N "you typed in 'lisp'. If you ara **) 
(princ “under VMS. type 'SHELL' to enter") 

(mag N "into EUNICE. If not. juat type 'liap'") 
(princ " again. " ) 

(tarpri ) 

(return (exit))) 

(t (tarpri) 

(mag N “Plaaaa enter 1. 2 or 3 only") 

(go loopl) 

> 

> 

) 

) 

) 

(cond ((equal anawer 2) 

(cl ear .display ) 

(mag N "Loading GENIE " N ) 

(include /u/gait/hanr ik/genle) 

(aat_up_framaa_f rom.f lie 'eimxpert. 1) 

(return (genie)))) 

(cond ((equal anawar 3)(return nil))) 

(cond ((equal anawar 2) (return nil))) 

(cond ((equal anawar lHraturn nil)) 

(t 

(tarpri > 

(mag N "Plaaaa enter 1. 2 or 3 only") 

(go loop] 

ii clear.diaplay atanda for clearing the diaplay on VT100 type terminal, 
(defun clear.dlaplay ( ) 

(princ (aacii 27)) 

(princ (aacii 91)) 

(princ (aacii 72)) 

(princ (aacii 27)) 

(princ (aacii 91)) 

(princ (aacii SO)) 

(princ (aacii 74] 


ii Running 'atartup ' function 
(clear_diaplay > 

(atartup > 


0 






(value_input_rb tparams_in_if s ( ini tial_value < equal_theta_val ue*_f or_all_axe* 

(rule (value_input_vb_rule22> 

(value_input_rb_rule23> > ) 
(equal_omega_value*_for_£ll_a»e* 

(rula (value_input_rb_rule26> 

(value_inpot_vb_rule27) > > 

(f ir«t_»tep_f ound 
(rula (valua_input rb_rule3> > > 

. (del taT 

(rula (valua_input_rb_rule3> >) 
(mtaadg_stata_arror 
(rula (value_input_rb_rula3> > > 

( *acond_*tep_f ound 
(rula (value_input_rb_rule3) ) ) 

( i«ip_amp_f ound 

(rule (vaiue_input_rb_rule3> > ) 

( ini t_thata»0_uanted 
(rule < velue_input_rb_rul*21 > ) > 

( integration_method 
(rula (value_tnput_rb_rula29) 
(value_input_rb_rule2) > ) 
(teontrol_l 

(rula (value_input_rb_rule24> > > 
(tcontrol_2 

(rule (value_input_rb_rula24J> ) 
(tcontrol_3 

(rula (valua_input_rb_rule24) > ) 
(ocontrol_l 

(rula (valua_input_rb_rula24> > > 
(ocontrol_2 

(rule (valua_input_rb_ruie24> ) ) 
(ocontrol_3 

(rula (valua_input_rb_rula24> > ) 

( qua tern ion_ini tial i zed 
(rula (value_input_rb_rul.e2> ) ) 
(axi*_of_input_command 
(rula (value input_rb rula2)>) 

(TO 

(rula (value_input_rb_rule2) ) ) 

(error 

(rula (value_input rb rule2))> 
(Y_matri*_full 

(rule (valua_input_rb_rula2) > ) 

( init_omaga»0_uiantad 
(rula (valua_input_rb_rule23> > > 
(#rame_full 

(rule (value_input_rb_rulei > > ) 
(amplitude 

(rula (valua_input_rb_rula4) > ) 

( ini t_f requency_value 
(rula (value_input_rb_rula4) > > 
(fraquencg_daltaT 
(rule (value_input_rb_rule4) > ) 

( numb er_of_d* cade* 

(rula (value_input_rb_rula4> ) > 
(numb*r_of_*ampl ing_frequencg_par_dacade 
(rula (valua_input_rb_ruie4) > > 

(phase 

(rula (valua_input_rb_rule4> ) ) > 
(Kp_matrix (matri »_*gmm*tr ic 

(rule (value_input_rb_rulel4) 
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(valu*_input_rb_rul8l&> > > 

(matr lx_typ* 

(rul* ( valu*_input_rb_rul*18> > > 

(*qual_diagonal_t*rmf 
(rul* ( valu*_input_rb_rul*18> ) ) 

(matrix_full 

(rul* (valu*_input_rb_rul*2> > ) ) 

(Kd_matrix (matr i x_*gmm* tr lc 

(rul* ( valu*_input_rb_rui*17> 

(valu*_lnput_rb_rul*19> > > 

(matri x_typ* 

(rul* (valu*_input_rb_rul#19) ) ) 

( *qua l_d lag onal_t*rm« 

(rul* (valu*_input_rb_rul*19> > > 

(matrix.full 

(rul* (valu*_inpub_rb_rul*2) > ) ) 

( control l*r_tup*_d**ir*d ( tam*_d 1 f f *r*nt la l_control_f or_al l_ax** 

(rul* (vaiu*_lnput_rb_rul*10) 

(valu*_input_rb_rul*l2> > ) 

(d*r lvativ* 

(rul* 

(valu*_input_rb_rui*9> > ) 

(proportional 

(rul* 

(valu*_input_rb_rul*6> ) ) 
(»am*_proportionai_control_f or_all_ax*« 
(rul* (vaiu*_lnput_rb_rul*9) 

< valu*_input_rb_rul*l 1 ) > > 


£ i <«« *tar t back on th* l*ft ««< 
(di#f_cro»*_coup_b*t_ax** 

(rul* (valu*_input_rb_rul* 8 > 

4 (valu*_input_rb_rul* 10 ) 

(valu*_input_rb_rul*17> 
(valu*_lnput_rb_rul* 12 ) 
g (valu*_input_rb_rul*15) ) ) 

i »»> continu* on th* right »»> 


i ««< start back on th* left <«« 

(prop_cro«s_coup_b*t_ax*« 

(rul* (valu*_input_rb_rul*7) 

(valu*_input_rb_rul*9) 

(valu*_input_rb_rul*ll ) 

(valu*_input_rb_rul*14) 

(valu*_input_rb_rul*16) ) ) 
i »»> continu* on th* right »>» 

) 

(Kd ((1 1 ) 

(rul* (valu*_lnput.jrb_rul* 10 ) 
(valu*_input_rb_rul*t7> 
(valu*_input_rb_rul* 12 ) 
(valu* input_rb_rul*lS> ) > 

((2 2 ) 

(rul* (valu*_input_rb_rul*17> 
(valu*_input_rb_rul* 12 ) 
(valu* input_rb rul*19>>) 

((3 3) 

(rul* (valu*_input_rb_rul*17) 
(valu«_input_rb_rul* 12 ) 
(valu*_input_rb_rul*15> ) > 


* 


t 



(a 2> 

(rul* (valu*_input_rb_rul*i7) 

(valu* Input rb rul*i 9 )>) 

((1 3) 

(rul* (v*lu*_lnput_rb_rul*17> 

(v«lu*_lnput_rb rul*19)>) 

((2 3) 

(rul* (valu*_input_rb_rul*17) 

(valu*_input_rb_rul*13) ) ) • 

((21) (rul* (v*lu*_input_rb_rul*17) ) ) 
((3 1> (rul* (v*lu*_tnput_rb_rul*17) > ) 
((3 2) (rul* (valu* Input rb_rul*17> ) > ) 
(Kp ((1 1) 

(rul* (valu*_input_rb_rul*9> 
(valu*_input_rb_rul*ll ) 
(valu*_input_rb_rul*14) 
(valu*_lnput_rb_rul*16) ) ) 

((2 2 ) 

(rul* (valu*_input_rb_rul*ll) 
(valu*_input_rb_rul*14) 
(valu*_lnput rb rul*16)>) 

((3 3) 

(rul* (valu*_lnput_rb_rul*ll > 
(valu*_input_rb_rul*14> 
<valu*_input rb rul*16)>) 

((1 2 ) 

(rul* (valu*_input_rb_rul*14> 

(valu* input rb_rul*16>>) 

((1 3) 

(rul* (valu*_input_rb_rul*14> 

(valu*_input rb rul*16)>) 

((2 3 ) 

(rul* (valu*_input_rb_rul*14) 

(valu*_input_rb_rul*16) ) ) 

((2 1) (rul* (valu*_input_rb_rul*16> ) ) 
((3 1> (rul* (valu*_input_rb_rul*16) ) ) 
((3 2) (rul* (valu*_lnput_rb_rul*16> ) ) ) 
(r*«pon**_chot*n 
(r*spons*_tgp* 

(rul* (valu*_input_rb_rul*3) 

(valu*_input_rb_rul*2) ) > ) 

(Y_fl>atri* (th*ta_x 

(rul* (valu*_lnput_rb_rul*22) 

(valu*_lnput_rb_rul*23) ) ) 
(om*ga_x 

(rul* (valu*_lnput_rb_rul*26) 

(valu*_input_rb_rul*27) ) ) 

( th*ta_g 

(rul* (valu*_lnput_rb_rul*23) ) ) 
(th*ta_z 

(rul* (valu*_lnput_rb_rul*23> ) ) 
(om*ga_g 

(rul* (valu*_input_rb_rul*27> > > 
(om*ga_z 

(rul* ( valu*_input_rb_rul*27> ) > ) 
(quaternion (want*d_bg_u**r 

(rul* (valu*_input_rb_rul*20) ) ) 
(roll_angl* 

(rul* (valu*_input_rb_rul*20) ) > 
(pitch_angl* 

(rul* (valu*_lnput_rb_rul*20) ) ) 



* (yau_angle 

(rule (valu*_input_rb_rulo2Q) ) )> 

( lnertial_jnat ” 

(matrl«_full (rule (valu*_input_rb_rul*2> ) ) ) 

( inert ial_matr ix 

(<1 1) (rul* (valu*_input_rb_rul*13) ) ) > ) 

(if_r*mov*d (d_r*mov*_rul*s ) ) 

(cr*at*d_bg (ISamir> Juan, and Andyl)) 

(cr*at*d_on (I6_13_83l)) 

(rules (valu*_input_rb_rula3> 

(valu*_input_rb_rul*3) 

(valu*_input_rb_rul*6) 

(valu*_input_rb_rul*18) 

(valu*_input_rb_rul*19) 

(valu*_input_rb_rul*21 ) 

(valu*_input_rb_rul*22) 

(valus_input_rb_rul*23) 

(value_lnput_rb_rula26) 

(valu*_input_rb_rul*27) 

(valua_input_rb_rula29) 

(valu*_input_rb_rul*20) 

(valua_input_rb_rula24) 

(valu*_input_rb_rul*8> 

(valu*_input_rb_rul*10) 

(valua_input_rb_rulal7) 

(valu*_input_rb_rul*12) 

(valu*_input_rb_rul*2) 

( valu*_input_rb_rul*23) 

(valu*_input_rb_rul*l ) 

(valua_lnput_rb_rul*7) 

(valu*_input_rb_rule9> 

(valua_lnput_rb_rulall) 

(valua_lnput_rb_rulal4) 

( valu*_lnput_rb_rulal6) 

(valu*_input_rb_rul*4) 

( valu#_input_rb_rul*13) 

(valua_lnput_rb_rulal3) ) 

(param_specs (fram*_full (value_type (lltaral)) 

(param_cf (no)) 

(f lnd_strategy (trg_rulas) (ask))) 

(mod* (value_type (lltaral)) 

( valua_mul tipllc ltg (single)) 

(value_r*qulred (gas)) 

(param_cf (no)) 

( lagal_valuas (step) (frequency))) 

(tcontrol_l (f ind_strat*gg ( trg_rules ) ) ) 

(tcontrol_2 (f lnd_strat*gg <trg_rules) ) > 

(ocontrol_l (f ind_strategy (trg_rul*s ) > ) 

(oeontrol_2 ( f ind_strat*gg ( try_rules ) ) > 

(lmp_amp_ok (value_tgpe (literal)) 

(value_fflultlpllcity (single)) 

(valu*_required (gas)) 

( legal_values (gas) (no)) 

(proflip t_format (raenu_input ) ) 

(message_f ormat (print 

IDo you want the calculated value of 'Impulse amplitude' 71) 
(print 

Ilf you sag N0» then you can give your own value. I))) 
(axis_of_input_command (value_mul tipllclty 

(single) ) 

(value_requir*d (yes)) 



(prompt_format 
<mcnu_input> ) 

( 1 e g a 1 _va 1 u e 1 (X) 

(Y) 

<Z> 

(none) ) 


> 


t 


* 


> 


h 


I 


» 

» 


0 


1 


» 


i <«« «tart back on the lift «<« 

(meieage_f or mat (print 

lYou can apply the input command to only on* alia or none of theml) 
(print IPleate select your choice (only one)l)> 
i »»> continue on the right »»> 

)) 


( param_c one lue lone ( inlt ial_value (frame_full 

(rule 

(value_input_rb.juie3> > > 
(tcontrol_l 
(rule 

(value_input_rb_ruie21 ) > > 
(tcontrol _2 
(rule 

(value_input_rb_rule22) ) ) 
(tcontrol_3 
(rule 

(value_input_rb_rule23) 1 ) 
(ocontrol _2 
(rule 

(value_input_rbjrule26) ) ) 
(ocontrol_3 
(rule 

(value_input_rb_rule27) ) ) 
(error 
(rule 

(value_lnput_rb_rule29) > ) 
(quaternion_initial lzed 
(rule 

(value_input_rb_rul#20) > > 

( ini t_theta*0_uanted 
(rule 

(value_input rb rule20>)> 
(Y_piatri*_full“ 

(rule 

(value_input_rb_rule24) 1 1 
(f ire t_*tep_f ound 
(rule 

(value_input_rb_rule2> > > 
(ocontrol_l 
(rule 

(value_input_rb_rula2S> > > 
(eecond_etep_f ound 
(rule 

(value_input_rb_rule4) > > > 
(Kd_jnatri« (matrii_typo 

(rule (value_input_rb_rule3) 
(value_input_rb_ruieB) 
(value_input_rb_rulel7> 
(value_input_rb_ruielS) ) ) 
(equal_diagonal_term* 

(rule (value_lnput_rb_rulelO> 

(value_input_rb_rulel2> > > 
(matr ix_ful 1 



(Kp_ir.atrix 


(rul# (value_input_rb_rul#3> 

(valu»_input_rb_rulol9> ) ) ) 
(matr ix_typ# 

(rul# (valu#_lnput_rb_rul#6) 
(valu#_input_rb_rul#7> 

( valu#_input_rb_rul#14) 

( valu#_lnput_rb_rul#16) ) ) 
(#qual_di«gonal_t»rm» 

(rul# (valu#_input_rb_rul#9> 

(valu#_input_rb_rul#ll > ) > 
(matrlx_full 

(rul# (valu#_input_rb_rul#6> 

(valu# Input rb rulalB)))) 

(Kp ((12) 

(rul# (valu#_input_rb_rui#6) 

(valu»_lnput rb rul#7))> 

((1 3) 

(rul# (valu#_lnput_rb_rul#6> 

(valu#_input rb_rul#7)>> 

((2 1 ) 

(rul# (valu#_input_rb_rul#6) 
(valu#_lnput_rb_rul#7) 
(valu#_lnput rb rul#14>>) 

((2 2 ) 

(rul# (valu#_input_rb_rul#6) 

(valu#_lnput rb_rul#9>)> 

((2 3) 

(rul# (valu#_input_rb_rul#6) 

(valu#_input rb_rul#7))> 

((3 1) 

(rul# (valu#_input_rb_rul#6) 
(valu#_input_rb_rul#7) 
(valu#_input_rb_rul#14) ) ) 

((3 2) 

(rul# (valu#_input_rb_rul#&) 
(valu#_lnput_rb_rul#7) 
(valu#_lnput_rb_rul#14) > ) 

((3 3) 

(rul# (valu#_input_rb_rul#6> 

(valu#_input_rb_rul#9> ) > 

((1 1) (rul# (valu# input rb rul#&))>) 
(Kd ((1 2) 

(rul# (valu#_input_rb_rul#3) 

( valu#_lnput rb_rul#8> ) > 

(( 13 ) 

(rul# (valu#_input_rb_rul#3) 

(valu# Input rb rul#8))) 

((2 1 ) 

(rul# (valu#_input_rb_rul#3> 
(valu#_input_rb_rul#8) 
(valu#_lnput rb rul#13)>> 

((2 2 ) 

(rul# (valu#_input_rb_rul#3> 

( valu#_input_rb rul#10))> 

((2 3) 

(rul# (valu#_input_rb_rul#3) 

(valu#_lnput rb rul#8))> 

((3 1) 

(rul# (valu#_input_rb_rul#3> 
(valu#_lnput_rb_rul*B) 
(valu#_input_rb_rul#13) ) > 



I 


. - ((3 2> 

(rulo (valua_lnput_rb_rule3> 

( valua_lnput_rb_rulaB) ( 

(valua_input_rb rulalS))) 

((3 3) 

(rula (valua_lnput_rb_rule3> ( 

(value_input_rb_rulelO> ) > 

C < 1 l) (rula (valua_input_rb_rula3) ) ) ) 

(Y_matri* (thata_y i 

(rula (valua_lnput_rb_rula21 ) 

( valua_lnput_rb_rula22) ) > 

(thata_z , 

(rul* ( valua_input_rb_rula21 ) 

( value_input_rb_rula22) ) ) 

(omaga_y v 

(rula ( valua_lnput_rb_rula26> 

(valua_tnput_rb_rula23) ) > 

(omaga_i 

(rula (valua_lnput_rb_rula26) 

(valua_input_rb_rule23) ) ) 

<thata_* 

(rula (valua_input_rb_rula21 ) ) ) 

(omaga.i 

(rula (valua_lnput_rb_rula23) ) ) ) 

(inartlal_rnatrix ((3 3) 

(rula 

(valua_input_rb_rulal3) ) ) 

((2 2 > 

(rula 

(valua input_rb_rulal3> ) ) 

((1 2 ) 

(rula 

(valua input_rb_rulel3) ) ) 

((1 3) 

(rula 

(valua lnput_rb_rulal3) ) ) 

((2 1 ) 

(rula 

(valua input_rb_rulal3> ) ) 

((2 3) 

(rula 

(valua Input rb_rulal3)>) 

((3 1) 

(rula 

(valua input_rb_rulal3) ) ) 

(O 2) 

(rula 

(value_input_rb_rulel3) ) ) ) 

( inart ial_mat 
<matriz_full 

(rula ( valua_input_rb_rulal3) ) ) ) ) ) 

(valua_input_rb_rula3 (*typa (ifany)) 

(tthan (concluda ( ini tial_valua frama_full> trua)) 

(creatad_bg (*amlr>) ~ i 

(creatad_on (I2_13_83l)> 

(*if (land (tripla ( inltial_valua f ir*t_atep_found ) 
trua) 

(tripla (raaponaa_choaan raaponaa_tgpa) 

“ i 

IStap racponaal) 



(•not (triple ( inltial_valuo deltaT) m nil)) 
(•not 

(triple ( ini tial_value ttaady_atate_error ) 

m 

nil))) 

(•and (triple (retponte_chotan reepont a_type ) 

(Frequency retpontel) 

(triple ( ini t ial_value f ir»t_itep_f ound ) 


true) 

(triple ( ini tial_value tecond_ttep_f ound ) 
true) ) 

(•and (triple (retponte_choten retpont e_type > 
llmpulte retpontel) 

(triple ( ini t lal_value f irtt_ttep_f ound > 
true) 

(•not (triple ( initial_value deltaT) - nil)) 

(triple ( ini tial_value imp_amp_f ound > 


(value_input_rb_rule3 


(val ue_input_rb_.ru le6 


true ) > ) 


(mod if ied_by ((Juan J. Rodr lguei-Motcoto I ) ) 

(modified on (I6_9_83l)>) 

(•type ( ifal 1 ) ) 

(created by (tamlr)) 

(created_on (I3_12JB9I>) 

(•if (triple (controller_type_deeired derivative) " no>> 
(•then (conclude (Kd_matrix matri«_full) true) 

(conclude (Kd_matrix matrlx_type) xero) 


(conclude 
(conclude 
(conclude 
(conclude 
(conclude 
(cone lude 
(conclude 
(conclude 
(conclude 


(Kd (1 1)> 0.0) 
(Kd (1 2)) 0.0) 
(Kd <1 3)) 0.0) 
(Kd (2 D) 0.0) 
(Kd (2 2)) 0.0) 
(Kd (2 3) > 0.0) 
(Kd (3 1>> 0.0) 
(Kd (3 2)) O. 0) 
(Kd (3 3)) 0.0) 


(claute_prlnt 

(All valuet of the Kd Matrix have been found. I)) 


(modified _by (I Juan J. Rodriguet-Motcoto I ) ) 
(modified on ( 16 9 831 ) ) ) 

(•type ( ifal 1 ) ) 

(created_by (tamlr)) 

(created.on (13 12 830) 

(•if 


(triple (controller. 

_type_detlred proportional) “ no)) 

(•then (conclude 

(Kp 

jnatrix 

matrix_full) 

true) 

(conclude 

(Kp 

matrix 

matrix_type) 

zero) 

(conclude 

(Kp 

(1 

1>> 

0 . 0) 


(cone lude 

(Kp 

(1 

2)) 

0 . 0) 


(conclude 

(Kp 

(1 

3)) 

0 . 0) 


(conclude 

(Kp 

(2 

1) ) 

0 . 0) 


(conclude 

(Kp 

(2 

2) > 

0 . 0) 


(conclude 

(Kp 

(2 

3) > 

0.0) 


(conclude 

(Kp 

(3 

1>> 

0 . 0) 


(conclude 

(Kp 

(3 

2) ) 

0 . 0) 


(conclude 

(Kp 

(3 

3)) 

0 . 0) 




(clause _pr int 

(All values of the Kp Matrix have been found. I)> 

(modif ied_by (I Juan J. Rodr iguez-Moscoso I > ) 

(modif ied_on (I6_9_85l>)> 

(value_input_rb_rulel8 (Stype ( ifany >> 

(•then (conclude (Kp_matrix matrix_full) true)) 

(created_by (samir)) 

(created_on (I3_13_89l)) 

(•if (*and (triple (Kp_matrix matrix_type) - diagonal) 

(triple (Kp_matrlx equal_d iagonal_terms > 

true) ) 

(•and (triple (Kp_matrix matrix_type) - diagonal) 

(triple (Kp_matrlx equal_dlagonal_terms) 

m 

false) ) 

(triple (Kp_matrix matrix_type) - symmetric) 

(triple (Kp_matrix matrix_type) - regular)) 

(modif ied_by 

(I Juan J. Rodr iguez-Moscoso ti Bor-Jau Hsieht)) 

(modif led_on (I6_9_8SI))) 

(value_tnput_rb_rulel9 (*type (ifany)) 

(•then (conclude (Kd_matrlx matriz_full) true)) 

(created_by (samir)) 

(created_on (13_19JB3I)) 

(•if (*and (triple (Kd_matrlx matrlx_type) ■ diagonal) 

(triple (Kd_matrlz equal_diagonal_terms) 

■I 

true) ) 

(•and (triple (Kd_matrlx matrii_type) • diagonal) 

(triple (Kd_matrix equal_diagonal_terms) 

m 

false) ) 

(triple (Kd_matrix matrix_type> “ symmetric) 

(triple (Kd_matrlx matriz_type) ■ regular)) 

(mod if ied_by 

(IJuan J. Rodr iguez-Moscoso ti Bor-Jau Hsieht)) 

(modified_on (I6_9_8SI)>> 

(value_input_rb_rule21 (•type (lfall)) 

(created_by (samir)) 

(created_on (I4_30_89D) 

(•if (triple ( ini t lal_value lnlt_theta>0_wanted) » yes)) 

(•then (conclude ( ini t lal_value tcontrol_l) true) 

(conclude (Y_matrix theta_x) 0.0) 

(conclude (Y_matrlx theta_y) 0.0) 

(conclude (Y_matrix theta_z) 0.0) 

(clause_prlnt 

IThe angular position for all axes have been initialized. I>) 
(modif led_by 

(IJuan J. Rodr iguez-Moscoso li Bor-Jau Hsiehl)) 

(modif ied_on (I6_9_89l))) 

(value_input_rb_rule22 (Stype (ifall)) 

(created_by (samir)) 

(created_on (I4_30_BSD) 

(•if (triple ( ini tial_value 

eyual_theta_values_f or_al l_axas ) 

y es ) 

(clause_print 

IThe common value of angular position for all axes> Theta(x)> isl) 
(c lause_pr int required.) 


* 


l 


I 


I 


l 


I 


i 


i 


i 


i 


i 



(•not (triple (Y_matrix theta_x) “ nil))) 

(•then (conclude ( ini t ial_value tcontrol_2> true) 

(conclude (Y_matrix theta_g) (Y_matrix theta_x)) 

(conclude (Y_matrix theta_x) (Y_matrlx theta_*)) 

(clause_prlnt 

(Angular position has been initialized. I > ) 

(mod if ied_bg 

(I Juan J. Rodriguez-Moscoso *» Bor-Jau Hsiehl)) 

(mod if ied_on (16 9_B9I>>) 

(value_input_rb_jrule23 (*tgpe (ifall)) 

(created_bg (samir)) 

(created_on (I4_30JB9I)) 

(•if (triple ( ini t ial_value 

equal_theta_values_f or_al l_azes ) 

no) 

(clause_print 

(Values for angular position (Theta) along all axes are required. I> 
(•not (triple (Y_jnatrix theta_x) - nil)) 

(•not (triple (Y_matrix theta_g) ■ nil)) 

(•not (triple (Y_matrix theta_z) ■ nil))) 

(•then (conclude ( ini tial_value tcontrol_3) true) 

(clause_print 

(Angular position has been initialized. I > ) 

(modified_bg * 

(I Juan J. Rodriguez-Moscoso & Bor-Jau Hsiehl)) 

(modlfled_on (I6_9_851))) 

(value_input_rb_rule26 (*tgpe (ifall)) 

(created_bg (samir)) 

(created_on (I4J30JBSO) 

(•if (triple (initial_value 

equal_omega_values_f or_al l_azes ) 

m 

ges) 

(clause_print 

IThe common value of angular ratei Omsga(x). is required, t) 

(•not (triple (Y_matrix omega_z) * nil))) 

(•then (conclude ( init ial_value ocontrol_2> true) 

(conclude (Y_/natrix omega_g) (Y_matriz omega_z>) 

(conclude (Y,jnatrix omega_z) (Y_matrlz omega_z>) 

(clause_print 

IThe angular rate has been initialized. !>) 

(modif led_.bg 

((Juan J. Rodriguez-Moscoso ti Bor-Jau Hsiehl)) 

(modif ied_on (I6_9_8SI))> 

(value_input_rb_rule27 (*tgpe (ifall)) " 

(created_bg (samir)) 

(created.on (I4_30JBSI)) 

(•if (triple ( lnitial_value 

equal_omega_values_for_all_axes) 

m 

no) 

(clause ..print 

(Values for angular rate along all axes are required. I ) 

(•not (triple (Y_matrix omega_x) ■ nil)) 

(•not (triple (Y_matrix omega_g) “ nil)) 

(•not (triple (Y_matrlx omega_z) ■ nil))) 

(•then (conclude ( ini t lal_value ocontrol_3) true) 

(clause_print 

(The angular rate has been initialized. I)) 

(modif ied_.bg 



• ' < I'Juan J. Rodriguez-Motcoto fc Bor-Jau Hciehi)) 

(modif ied.on ( 16.9.831 )>) 

(value.input.rb rule29 (Stgpe (ifall)) 

(•if 

(triple ( ini tial.value integration.method ) - Euler)) 

(•then (conclude ( ini t iel_velue error) O.O)) 

(created.bg (IBor-Jeu Heieh & Juan Rodriguez-Moscoso I ) ) 

(created_on (16.12 831))) 

" (value_input_rb_rule20 (*tgpe (ifall)) 

<*if (triple (quaternion wanted.bg.user ) ■ gee) 

(•not (triple (quaternion roll_angle) - nil)) 

' (*not (triple (quaternion pitch_angle> “ nil)) 

(•not (triple (quaternion gaw.angle) - nil))) 

(Seise 

(conclude ( ini t lal_vaiue quaternion_lnltiallzed > true)) 

(created.bg (samir)) 

(created.on (14.29.831)) 

(*then (conclude ( init ial.value quaternion.lnltlallzed) 
true) 

(conclude ( ini tial.value ini t_theta«0_uanted ) 
gee) 

(clause_print 

(The Rolli Pitch, and Yaw angles have been initialized. I>) 
(modified_.bg (I Bor-Jau Hsieh & Juan Rodr iguez-Moscoso I ) ) 

(mod if ied.on (16.12 831))) 

(value_input_rb_rule24 (*then (eonclude“(initial_value Y_matri«_full) true)) 

(created.bg (samir)) 

(created.on (14 30 831)) 

(•tgpe (ifall)) 

4 (»if (clause_print 

(Initial values for angular rate and/or angular position are required! > 
(•or (triple ( init ial.value tcontrol.l) ■ true) 

(triple ( init ial.value tcontrol_2) ■ true) 

(triple ( ini tial.value tcontrol_3) " true)) 

(•or (triple ( init ial.value ocontrol.l) ■ true) 

(triple ( ini tial.value ocontrol_2) “ true) 

(triple ( ini tial.value ocontrol_3) “ true))) 

(modifled.bg (likes Tflnal value)) 

(mod if led.on ( (6.14.831 )) > 

(value.input.rb.ruleS (Stgpe (ifall)) ” 

(creatad.bg (samir)) 

(created.on (13.12.830) 

(•then (conclude (Kd matrix matrix tgpe) diagonal) 

(conclude (Kd (1 2)) 0.0) 

* (conclude (Kd (1 3>> 0.0) 

(conclude (Kd (2 1)) 0. 0) 

(conclude (Kd (2 3)) 0.0) 

* (conclude (Kd (3 1)) 0.0) 

(conclude (Kd (3 2>> 0.0)) 

(•if 

* (triple (controller.tgpe.deslred 

diff_cross_coup.bet.axes) 

* no) ) 

(modif ied.bg (likes Tflnal value)) 

(modif led.on (16.14.831))) 

, ( value.input.rb.rulelO (Stgpe (ifall)) 

(created.bg (samir)) 

(created.on ( (3.12.831 )> 

, (Sthen (conclude (Kd.matrix equal diagonal terms) true) 

(conclude (Kd (2 2)) (Kd (I 1))) 



‘ (conclude (Kd (3 3)) (Kd (1 1))> 

(c lause_pr int 

(All value* of the Kd Matrix have been found. I)> 

(•if (triple (controller _type_desired 

dif f_cr oss_coup_.be t_axes) 

no) 

(triple (control ler_type_deslred 

«ame_dif ferential_control_f or_all_axe* ) 

yes ) 

(clause_print 

IThe value of the Kd Controller Gain Matrix common to all axes!) 
(clau*e_print II* required. I) 

(•not (triple (Kd (1 1)) - nil))) 

(modified_by (like* Tf inal_value) ) 

(mod if ied_on ( I6_14_83l ) ) ) 

(value_input_rb_rul*17 (•type (lfall)) 

(created_bg («amir>) 

(created_on (I3_14_83l)) 

(•then (conclude (Kd_matrix matrix_tgpe) regular) 

(clau*e_print 

I A 1 1 values of the Kd Matrix have been found. I>) 

(•if (triple (control ler_type_deslred 

dif f_cro**_coup_bet_axe*> 

ge* ) 

(triple (Kd_matrix matr ix_*ymm*tr ic > ■ no) 

(clau*e_print ~ 

(Since Kd i* neither symmetric nor diagonal all 9 of it* values I ) 
(clause_print 

lare required. As you see them prompted for< please enter values. I) 


(•not 

(triple 

(Kd 

(1 

1 ) ) 

m 

nil)) 

(•not 

(triple 

(Kd 

(1 

2)) 

m 

nil)) 

(•not 

(triple 

(Kd 

(1 

3) ) 

m 

nil)) 

(•not 

(triple 

(Kd 

(2 

m 

m 

nil)) 

(•not 

(triple 

(Kd 

(2 

2)) 

m 

nil)) 

(•not 

(triple 

(Kd 

(2 

3)) 

m 

nil)) 

(•not 

(triple 

(Kd 

(3 

in 

m 

nil)) 

(•not 

(triple 

(Kd 

(3 

2)) 

m 

nil)) 

(•not 

(triple 

(Kd 

(3 

3) ) 

m 

nil))) 


(mod if led_by ( 1 i kes_Tf inal_value > ) 

(modif led_on (I6_14_83l))> 

(value_input_rb_rulel2 (*type (lfall)) 

(created_by (samir)) 

(created_on (I3_12_83l)) 

(•then (conclude (Kd_matrlx equal_dlagonal_terms) false) 

(clause_j>rlnt 

I A 1 1 values of the Kd Controller Gain Matrix have been found. I)) 
(•if (triple (eontroller_type_desired 

dif f_cros»_coup_bet_axes ) 

no) 

(triple (controller_type_de*ired 

same_dif f erent ial_control_f or_all_axes ) 

m 

no ) 

( c lau*e_pr int 

IThe different values of the Kd Controller Oaln Matrix along I > 
(clause_print 

Itha axes are required. Qains along the X. Y. and Z axes will! ) 



‘ ' ( c lause_pr int 

lb* prompted in that order. Please enter values. I) 

(♦not (triple (Kd (1 1)) - nil)) 

(♦not (triple (Kd (2 2>> - nil)) 

(♦not (triple (Kd O 3)) - nil))) 

(modified_by (likes Tf inal_value ) ) 

(modif ied_on ( I6_14_83I ) ) > 

(value_input_rb_rule2 (♦type (ifall)) 

(created_by (samir)) 

(created_on (I2_3_85l)) 

(♦then (conclude (initial_value f irst_step_f ound ) true)) 

(♦if (triple ( inertial_mat matrix_full) '■ true) 

(triple (Kp_matri x matrlx_full) ** true) 

(triple (Kd_matrlx matrlx_full) « true) 

(triple (lnitial.value quaternion_initialixed) 

m 

true ) 

(♦not 

(triple ( ini t ial_value lntegrat ion_method ) - nil)) 

(♦not 

(triple (response_chosen response_type> " nil)) 

(♦not 

(triple ( ini t ial_value ai is_of_input_command ) 
nil)) 

(♦not (triple ( ini t ial_value TO) " nil)) 

(♦not (triple ( ini tial_value error) ■ nil)) 

(triple ( ini t ial_value Y_matrix_full> ■ true)) 

(modif ied_by (ISemir. Andy# and Juanl)) 

(modif ied_on ( I6_13_83l > > > 

(value_input_rb_rule25 (♦if (triple ( ini t ial_value inlt_omega*0_wanted ) ■ yes)) 

(created_by (samir)) 

(created.on (14 30 831 >) 

(♦type (ifall)) 

(♦then (conclude ( lnlt ial_value oeontrol_l> true) 

(conclude (Y_matrix omega_x) 0.0) 

(conclude (Y_matrix omega_y) 0.0) 

(conclude (Y_matrlx omega_z) 0.0) 

(clause sprint 

(The angular rate has been initlaliied. I)> 

(mod if ied_by 

(I Juan J. Rodr iguez-Moscoso It Bor-Jau Hsieht)) 

(modif ied_on ( I6_16_83l ) ) ) 

( value_input_rb_rulel (♦type (ifall)) 

(♦if (triple ( ini t ial_value frame_full) ■ true)) 

(created_by (samir)) 

(created_on (I2_3JB3D) 

(♦then (clause_prlnt 

I A 1 1 parameter values needed for the simulation program have been found. I) 
(c lause_pr int 

I By using option 3 of the TOP LEVEL MENUi the simulation program can bet) 
(c lauso_pr int run. )) 

(modlfied_by (IJuan J. Rodr iguez-Moscoso & Bor-Jau Hsiehl)) 

(modif ied_on ( I6_16JB3I ) > > 

( value_input_rb_rule7 (Otype (ifall)) 

(created_by (samir)) 

(created_on (I3_12_B3I)) 

(♦then (conclude (Kp_matrlx matrix.type) diagonal) 

(conclude (Kp (1 2)) 0.0) 

(conclude (Kp (1 3) ) 0. 0) 

(conclude (Kp (2 1) ) 0. 0) 


0 



J (conclude (Kp (2 3)> 0.0> 

(conclude (Kp (3 1)) O.O) 

(conclude (Kp (3 2)) 0.0)) 

(•if 

(triple (controller_type_deslred 

prop_croee_coup_bet_«*ee ) 


no) ) 

• (modif ied_by (I Juan J. Rodr iguex-Moscoso I ) ) 

(modi f ied_on (16 16 851))) 

(value_input_rb_rule9 (*type (ifall)) “ “ 

i (created_by (tamir)) 

(created_on (I3_12_85I)) 

(•then (conclude (Kp_matrlx equal_d iagonal_terms ) true) 

* (conclude (Kp (2 2>) (Kp (1 1>>> 

(conclude (Kp (3 3)> (Kp (1 1))) 

(c laut e_print 

k (All values of the Kp Matrix have been found. I>> 

(•if (triple (controller_type_deslred 

prop_cross_coup_bet_axes ) 

i ■ 

no) 

(triple (control ler_type_de»ired 

same_proportional_control_f or_all_axes> 


yes) 

• (clause_print 

I The common value of the Kp gain for all axes is required. It isl) 
( c lause_pr int (called Kp (1 l).l) 

, (•not (triple (Kp (1 1)) - nil))) 

(modlfied_by (IJuan J. Rodr iguei-Moscosol ) ) 

(modif led_on ( I6_16JB3I ) ) ) 

I (value_input_rb_ruleil (*type (ifall)) 

(created_by (samir)) 

(craated_on (I3_12_83D) 

, (•then (conclude (Kp_matrix equal_diagonal_terms ) false) 

(clause_print 

IAU values of, the Kp Matrix have been found. I>> 

(•if (triple (controller_type_desired 

prop_cross_coup_bet_axes) 

no) 

(triple (control ler_type_desired 

same_proportional_control_f or_ail_axes) 


no) 

(clause_jirint 

IThe values of the elements of the Kp Matrix along the axes arel) 
(clause_print required. ) 

(•not (triple (Kp (1 1)) ■ nil)) 

(•not (triple (Kp (2 2>) - nil)) 

(•not (triple (Kp (3 3)) - nil))) 

(modif led_by (IJuan J. Rodr lguex-Moscoso I ) ) 

(modif ied_on ( I6_16_83l ) ) ) 

(value_input_rb_rulel4 (*type (ifall)) 

(created_by (samir)) 

(created_an (I3_14_B3I>) 

(•then (conclude (Kp matrix matrix type) symmetric) 

(conclude (Kp (2 1>> (Kp (1 2))) 

(conclude (Kp (3 1)) (Kp (1 3))) 

(conclude (Kp (3 2) > (Kp (2 3))) 


I 


1 


(. 



» ; (clause_prlnt 

(All valuta of the Kp Matrix have been found. t>) 

(•if (triple (controller_type_desired 

prop_cro»»_coup_bet_axe*> 

yes ) 

(triple (Kp_matrix matrix_symmetric ) - yes) 

(clause_print 

ISince Kp is a symmetric matrix, only 6 of its values are required, t) 
(clause_pr int 

lAs you see them prompted for> please enter values. I) 

(•not (triple (Kp (1 1)) - nil)) 

(•not (triple (Kp (1 2)) - nil)) 

(•not (triple (Kp (1 2)) - nil)) 

(•not (triple (Kp (2 2)> - nil)) 

(•not (triple (Kp (2 3)) - nil)) 

(•not (triple (Kp (3 3)) - nil))) 

(modlfied_by (IJuan J. Rodriguez-Moscoso I ) ) 

(modlf ied_on < I6_16_85l ) > ) 

(value_input_rb_rulel6 (*type (lfall)) ~ 

(created_by (samir)) 

(created_on (I3_14JB3D) 

(•then (conclude (Kp_/»atrlx matrlx_type) regular) 

(clause _print 

I All values of the Kp Matrix have been found. I)) 

(•if (triple (controller_type_desired 

prop_cross_coup_bet_axes> 

EX 

yes ) 

(triple (Kp_matrlx matr ix_symmetric ) - no) 

(clause_pr int 

ISince Kp is neither symmetric nor diagonal, all 9 of its values!) 
(clause_print 

lare required. As you see them prompted for. please enter values. I) 

(•not (triple (Kp (1 1)) - nil)) 

(•not (triple (Kp (1 2>> - nil)) 

(•not (triple (Kp (1 3)) - nil)) 

(•not (triple (Kp (2 1)) » nil)) 

(Snot (triple (Kp (2 2)) - nil)) 

(•not (triple (Kp (2 3)) - nil)) 

(•not (triple (Kp (3 1)) - nil)) 

(•not (triple (Kp (3 2)) - nil)) 

(•not (triple (Kp (3 3)) - nil))) 

(modified by (IJuan J. Rodriguez-Moscoso I ) ) 

(modlf led_on ( I6_16_83l ) ) ) 

(value_input_rb_rule4 (Stype (lfall)) 

(created_by (samir)) 

(created_on (I2_13_B3I)> 

(•then (conclude ( ini t ial_value second_step_found ) true) 

(clause_j>rint 

(All parameter values for FREQUENCY response analysis have beenl) 
(clause_print found.)) 

(•if (clause_print 

(Initial parameter values for FREQUENCY response analysis are required. I) 
(•not (triple ( ini tial_value amplitude) “ nil)) 

(•not 

(triple ( ini t ial_value lnit_frequency_value> 
nil)) 

(•not 

(triple ( initial_value 



f r*qu#ncy_del taT) 

B 

nil)) 


(knot 

(triple ( ini tial_value number_of_decad*s ) ■•nil)) 

Knot 

(triple ( ini t ial_valu* 

number_of_sampllng_frequ*ncy_per_dacade> 

m 

nil)) 

(•not (tripl* ( ini t ial_value phase) - nil))) 

(mod if i*d_by ( I Juan J. Rodriguez-Moscoso I ) ) 

(modif ied_on ( I6_16_B3I ) ) ) 

(value_input_rb_rulel3 (»typa (ifall)) 

(crea ted_by (sarair)) 

(creatcd_on (I3_I3_B3|)> 

(•if (clause_prlnt 

IFor the present) tha Inertia matrix Is assumed to be diagonal. I) 
(claut*_pr int 

lit is also assumed all diagonal elements to be 'equal'. This!) 
(clause_print 

(assumption makes the 'system' uncoupled in nature. I) 

(clause_print 

(Please enter the common element of- tb* diagonal of tha Inertial I ) 
(clause_print Matrix. ) 

(•not (triple ( in*rtial_matrix (1 D) « nil))) 

(•then (conclude (lnertial_matrlx (3 3)) 

( in*rtial_matr i i (1 1))) 

(conclude (inertial_matrix (22)) 

(inertial_matrix (1 1))) 

(conclude ( inertial_mat matrix_full) true) 

(conclude ( lnertial_matrix (1 2) ) O.O) 

(conclude ( inertlal_matrix (1 3)) 0.0) 

(conclude (lnertlal_piatrix (2 1)) 0.0) 

(conclude (lnertial_matrix (2 3)) 0.0) 

(conclude ( inertial_matr ix (3 D) 0.0) 

(conclude (inertial_matrix (3 2)) 0.0) 

(clause_print 

IA11 values of the 'Inertia matrix' have been found. I)) 

(modified_by (I Juan J. Rodriguez-Moscoso I > ) 

(modif iad_on (I6_16 B3I>>) 

(valu*_input_rb_rul*13 (•type (ifall)) 

(cr*ated_by (samir)) 

(created_on (I3_14_B3I)) 

(•then (conclude (Kd_matrix matrix_type) symmetric) 

(conclude (Kd (2 1)) (Kd (1 2))) 

(conclude (Kd (3 1>> (Kd (1 3))) 

(conclude (Kd (3 2>) (Kd (2 3>>) 

(clause_print 

I All values of the Kd Matrix have been found. I>> 

(•if (triple (controller_type_deslred 

diff_cross_coup_b*t_exes) 

yes ) 

(triple (Kd_matrix matr i x_symmetric ) " yes) 

(clause_print 

(Since Kd is a symmetric matrix» only 6 of its values are required. I) 
(clause_print 

I As you see them prompted for> pleas* enter values. I) 

(•not (triple (Kd (1 1>> « nil)) 

(•not (triple (Kd (1 2)) » nil)) 



(*not (triple (Kd (1 3) > - nil)) 

(♦not (triple (Kd (2 2)) - nil)) 

(♦not (triple (Kd (2 3)) - nil)) 

(♦not (triple (Kd (3 3)) - nil))) 
(<nodiPied_by (IJuen J. Rodr iguei-Moecoeo I ) ) 
(mod i# ied_on ( I6_16_B3I ) > ) 



<fila_indax ( f rames . ( f i la_i.ndax (simxpart. 1 ) ) 

(param_tpact (simxpart. 1 ) > 

(sgstam_spacs ( s lmxpart. 1 > ) 

(valoe_input_rb (rulei.l)) 

(sim_axpartlaganda (slmxpart. 1)1 
< start_aganda <* lmxpart. 1 ) ) 

( top_leval_aganda (slmxpart. 1 ) > 

( top_laval_manu (•lmxpart. 1 ) ) 

(top_laval_control (■lmxpart. 1 ) ) 

(•taga_l_rb (•lmxpart. 1 > ) 

(changa_j>aram_control (slmxpart. 1)) 

(dlsp_init_val_control («lmxpart. 1)) 

(param_manu (slmxpart. 1 ) ) 

(di*p_lnlt_val_aganda (slmxpart. 1>) 

(disp_init_yal_rb (slmxpart. 1 ) ) 

(changa_param_aganda (slmxpart. 1)) 

(tast_rb (as. 1 ) ) 

( output_d lsp lag_manu (slmxpart. 1)) 

(output_di*play_control (slmxpart. 1)) 

(output _dlsplag_aganda (slmxpart. 1)) 

(output _dlsplag_rb (slmxpart.!)) 

(run_rb (slmxpart.!)) 

(run_aganda (slmxpart. 1 ) ) 

(usar_rb (usar. !)) 

( change_param_rb (changa. 1)) 

(changa_matrix_rb (changa. 1))) 

(filas (slmxpart. 1 (fila_indax) 

(sgstam_spacs> 

(param_spacs) 

(slm_axpart_aganda) 

(start_aganda) 

( top_laval_aganda) 

( top_l awe ljnanu > 

(top_level_control ) 

(staga_l_rb (rula_basa)) 

(changa_param_control ) 

(disp_init_val_control ) 

(param_manu) 

(disp_lnlt_val_aganda) 

(dlsp_init_val_rb (rula_basa>) 

(changa_param_aganda) 

(output_dlsplag_manu) 

(output_dlsplag_control ) 

(output_d isplag_agenda) 

(output_displag_rb (rula_basa)) 

(run_rb (rula_basa)) 

(run_aganda) ) 

(rulas. 1 (valua_lnput_rb (rula_basa) ) ) 

(usar. 1 (usar_rb (rula_basa) > ) 

(usarl. 1 (usar_test_rb (rula_basa> ) ) 

(changa. 1 (changa_param_rb (rula_basa)) 

(changa_matrlx_rb (rula_basa) ) ) ) ) 

(sgstam_spacs (weight* (dafault ((3 3)))) 

(param_daf aul t (messaga_f ormat (print 

IPlaasai antar tha following paramatar valua: I ) 
(path)) 

(prompt_#ormat (values)) 

(param_cf (no)) 

(valua_tgpa (numerical)) 

(valua_multlplicltg (single)) 

( value_required (gas)) 



‘ ‘ (f ind_strategy (fget) 

< try_al l ) 

(ask > 

(default) 

(unknown) ) ) 

(backup_commands (b) (back) (backup) (up) (^) (undo)) 

(halp_coiii'»ands (help) (?) (hlep) (ship)) 

(uhy_commandt (why) (why?))) 

(param_spacs (CF (default (1.0)) 

(unknown (0.0)) 

(value_type (numerical)) 

(range ( (>■ 0.0)) ((0 1.0)))) 

(TO (param_cf (no)) 

(message_format (print 

IThe time of application of the input signal* also known asl) 
(print 

Ithe initial time* is required. Usually its value is 0.01) 
(print seconds. ) 

(path ) ) ) 

(Tfinal 

(message_f ormat (print 

IThe time at which the 'simulation program' should stop computing)) 
(print 

(the state of the system is required. It is also known as Tfinal. I) 
(path))) 

(proportional (value_requlred (yes)) 

(msssage_f ormat (print 

IDo you want a proportional (P) type control?! > 

(print I (For P and PD type control) I )) 

(prompt_f ormat (menu_input) ) 

( legal_values (yes) (no))) 

(derivative (value_requlred (yes)) 

( legal_values (yes) (no)) 

(message_forma t (print 

(Do you want a differential (D) type control?! > 

(print I (For D and PD type control) I >> 

(prompt_format (menu_input) ) ) 

(number_of_sampl ing_f requency_per_decade 
i «<« start back on the left ««< 

(message_f ormat (print 

lEnter the number of sampling frequencies per decade! > 

(print Ifor FREQUENCY response!) 

(print (Normal value is 3 frequencies!)) 
i »»> continue on the right »>» 


(prompt_f ormat (values))) 

(number_of_decades (message_f ormat (print 

lEnter the number of decades of input signal for FREQUENCY response!) 

(print 

(Normal value is 3 decades! >> 

(prompt_f ormat (values))) 

(matrit_symmetrlc (value_required (yes)) 

( legal_values (yes) (no)) 

(message_format (print 

I Is the following matrix symmetric?!) 

(path ) ) 

(prompt_f ormat (menu_lnput ) ) ) 

(samejproportional_control_for_all_axes (value_required (yes)) 

(legal_values (yes) (no)) 

(message_f ormat (print 

IDo you want the same proportional controller gain along!) 



(print 

lall axes ?l > > 


(prompt_format 
(menu_input ) > ) 

( *ame_dif f erent ial_control_f or_al l_axea (value_re<iuired (yea)) 

(legal_value* (yea) (no)) 

(me**age_f ormat (print 

I Do you want the eame differential controller gain fori) 
(print 

lall axea ? I ) ) 

(prompt_f ormat 
(menu_lnput ) ) > 

(reaponae_type (value_type (literal)) 

(value_multiplicity (aingle)) 

( value_req.uired (yea)) 

( legal_valuea (IStep reaponael) (IFreyuency reaponaal)) 

(me*aage_f ormat (print 

IPleaae aelect from the following menu the type of Syeteml) 

(print 

Iraaponae. Pleaae make only one choice. I>) 

(prompt_f ormat (menu_lnput) ) > 

( likea_Tf inal_value (value_type (literal)) 

(value_multipllcity (aingle)) 

(valuejreyuired (yea)) 

(legal_valuea (yea) (no)) 

(meaaage_format (print 

IJuat above ia the value of final time (Tflnal) aa computed byl) 

(print 

(the ayatem. Do you think that thia value la what you want ?. I) 

(print 

Ilf you anawer no then you can provide a new value for Tflnal. f>) 

„ (prompt_format (menu_input ) ) ) 

(frame_full ) 

(uanted_by_uaer (value_type (literal)) 

(value_jnultipllcity (aingle)) 

(value_required (yea)) 

(legal_valuea (yea) (no)) 

(mo**age_f ormat 
(print 

IDo you want the Quaternion block to be Included in the elmulatlon?! > ) 

(prompt_f ormat (menu_input) ) ) 

(roll_angle (prompt_format (value*) > 

(value_multipllclty (aingle)) 

(meaaage_f ormat (print 

IPleaae enter initial roll angle (rotation about the X axle) I ) 

(print (Enter your value in radianal))) 

(pitch_angle (prompt_f ormat (value*)) 

(me**age_f ormat (print 

IPleaae enter Initial pitch angle (rotation about the Z axis) I ) 

(print lEntar your value in radianal >>> 

(yaw_angle (prompt_f ormat (value*)) 

(me**age_f ormat (print 

IPleaae enter initial Yaw angle (rotation about Y axle) I > 

(print (Enter your value in radianal >>) 

( init_theta»0_uanted (valua_type (literal)) 

(value_multiplicity (aingle)) 

(value_reyulred (yea)) 

(measage_f ormat (print 

IDo you want the initial value of angular poaition to be 0.0 fori) 

(print lall axa*?l>) 

(prompt_f ormat <menu_input) ) 



(legal_v«lu*s (yet) (no))) 

( in i t_omega**0_wanted (value_type (litoral)) 

(value_jnultlplicity (tingle)) 

( value_requlred (got)) 

( legal_valuet (go*) (no)) 

(mettago_f or mat (print 

IDo gou want tha initial valua of tha angular valocitg to ba O.OI) 

(print tall axes?! ) ) 

(prompt_f ormat (manu_input) ) ) 

(equal_theta_values_for_all_aies (valua_tgpa (litaral)) 

(valua_multlplicltg (singla)) 

( value_requlred (gas)) 

( legal_valuet (gas) (no)) 

(mestage_f or mat (print 

IDo gou want tha angular position (Thata) to hava tha tame initial I ) 
(print 

Ivalua for all axet?l)) 

(prompt_f ormat (manu_input ) ) ) 

(equaI_omega_valuet_for_all_axet (valua_tgpa (litaral)) 

(valua_fflultipllcltg (singla)) 

(valua_raquirad (gas)) 

( lagal_valuas (gas) (no)) 

(messaga_format (print 

IDo gou want tha angular valocitg (Omega) to hava tha sama initial I ) 
(print 

Ivalua for all axas?l)> 

(prompt_format (manu_input) ) ) 

( lntagrat ion_mathod (valua_tgpa (litoral)) 

(valua_multlpllcity (singla)) 

(valua_raquirad (gas)) 

( logal_valuas (Eulor) 

( IFourth-order Rungo-Kutta I ) 

(IPredictor corrector I )) 

(massaga_format 

(print 

IPlaasa. select one method of integration: I ) ) 

(prompt_f ormat (manu_input ) ) ) 

(deltaT (metsage_f ormat (print 

IPloasa> enter a valua for delta_T. This is the valua of tha tlmel) 

(print I increment in the simulation.))) 

(prompt_f ormat (values)) 

(range ((> 0. 0>) ((< 0. 1))>> 

(error (range ((> 0. 0>>) 

(message_f ormat (print 

(Enter tha precision wanted for multi-step integration solactad. I) 

(print 

(Normal values range from 0.0001 (normal precision) to l.E-10 I) 

(print ((double precision). I )) ) 

(uants_to_run_simulation (value_type (literal)) 

< value_multipl icity (single)) 

(valua_required (gas)) 

( lagal_valuas (gas) (no>> 

(massaga_f ormat (print) 

(print 

IDo gou want to run the Simulation Program?!) 

(print 

I - If gou sag N0< gou can go back and review parameter values. I) 

(print 

I - If YES. gou will be running the FORTRAN-77 simulation program. I)) 
(prompt_f ormat (menu_input > ) ) 

(stoady_state_error (message_format (print 



(Please* enter the steady state error in percentage. For example* I ) 
(print 

(type in 2.0 for a 2Z steady state error. 1)1 
(prompt_f ormat (values))) 

(amplitude (message_f ormat (print 

(Enter the Amplitude of tha input waveform for FREQUENCY response!) 

(print (Normal value is 1.0 radiant)) 

(prompt_f ormat (values))) 

(frequency_deltaT (promp t_f ormat (menu_input ) > 

(legal_values (256) 012) (1024)) 

(message_f ormat (print 

(Please enter a value for deltaT for frequency response. I) 

(print 

Ilf you choose 912 from the menu below deltaT will be flxedl) 

(print 

Ito 1/912 or 0.00199312 sec. I) 

(print 

(This value of 912 Is racoaunended. !>)) 

(phase (message_format (print 

(Enter the phase of the input waveform for FREQUENCY responsel) 

(print INormal value is 0.0 radiansl)) 

(prompt_format (values))) 

( init_frequency_yalue (message_f ormat (print 

(Enter the lowest frequency of input wavwform to be applied tol) 
(print 

(the system for FREQUENCY response I > 

(print 

(Enter your value in Hertxl) 

(print 

INormal value is 0.0199194 Hx corresponding to 0.1 rad/secl)) 
(prompt_f ormat (values))) 

(diff_cross_coup_bet_axes (value.multlpllcity (single)) 

(value_jrequired (yes)) 

(legal_values (yes) (no)) 

( mes sag e_f ormat 
(print 

(For differential control do you want cross-coupling between control axes?!)) 
(prompt_format (menu_lnput)> ) 

(prop_cross_coup_bet_axes (value_multlplicity (single) > 

(value_required (yes)) 

( legal_values (yes) (no)) 

(message_f ormat 
(print 

(For proportional control* do you want cross-coupling between control axes?!)) 
(promp t_f ormat (menu_input) ) ) 

(param_specs ) ) 

( s iro_exper t_agenda ((from 'Y_matrix)) 

((frem 'quaternion)) 

( ( frem 'control ler_type_desired ) ) 

((frem ' inertlal_mat ) ) 

((frem 'Kd_matrix)> 

((frem 'Kp_matrix)) 

((frem 'inertial_matrix)) 

((frem 'Kd)> 

((frem 'Kp)) 



((frem '-in i t ia l_va lu» > > 

( (reset_rul#_base 'value_input_rb ) ) 

((back ' (value_input_rb_rulel ) 

'value_input_rb ) ) > 

(start_agenda ((array cigi f lonum-b lock" &)) 

((array aigr flonum-block b ) > 

((array one_over_tau flonum-block 1)> 

( (set_up_frames_from_f lie 'rules. 1>) 

( ( set_up_f ram#s_from_f i le 'change. 1)) 

((include demonar. 1)> 

((array lnertial_matrlx flonum-block 3 3)) 

((array Kp flonum-block 3 3)) 

((array Kd flonum-block 3 3 > > 

((cfaal 'aiganval. o 
'_eigen_ 

'aigan 

"real-function" 

"-1F77” ) ) 

((include demon03. 1)) 

((include demon02. -1 ) > 

((include demonOl.l)) 

((include demonOO. 1)) 

( (terpr 1 ) ) 

( (terpri ) ) 

((mag N 

"This Expert Syetem provides an Intelligent Interface to a generic")) 
((mag M 

"simulation program for spacecraft attitude control problems. Below")) 
( (mag N 

“is a menu of the functions the system can perform. Control will”)) 
((mag N 

"rapeatedly return to this menu after executing each user request.")) 
((setq xxx (walt_f or_user > ) > 

( (start jfc_itsrate> ) ) 

^toP—level—egenda ( (reset_rule_baae 'stage_l_rb ) ) 

(tfrem 'top_level_cholce > ) 

( (n#nu_input 'top_lev#l_control ) ) 

((forward 'stage_l_rb ) > ) 

(top_level_menu (lExit to GENIE!) 

(ISet up initial parameter values!) 

(IRun simulation program!) 

((Display current parameter values required for simulation!) 

(IDlsplay outputs generated by simulation!) 

((Change initial parameter values required for simulation!) 

(IBet up initial parameter values to default values!) 

(IStore current parameter values in a disk file!)) 

(top_level_control ( ( c lear_d isp lay ) ) 

(print ) 

(print I ** Please make only one choice at a time eel) 

(prompt_specs (get_alternatives_from (top_level_menu) ) 

(put_deta_in (top_level_choice) ) 

(data_input ) ) ) 

(stage_l_rb (if_removed (d_r#move_rules ) ) 

(created_by (samir)) 

(created_on (!2_20_85!)) 

(rules (stage_l_rb_rulel ) 

(stage_l_rb_rule2) 

(stage_l_rb_rule5) 

(stage_l_rb_rule8) 

(stage_l_rb_rulel2) 

(stage_l_rb_rule7> 
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„(«tage_t_rb_rulelO> 

(*tage_l_rb_rule9) 

(*tage_l_rb_rule3> 

(« tag a_l_rb_ru 1*13) 

(«taga_l_rb_rulal 1 > 

(*taga_l_rb_rula6) 

(«taga_l_rb_rule4) > 

(param«_in_if c < top_laval_choica 
< top_laval_manu 
(rola (*taga_l_rb_rolal ) 

(*taga_l_rb_rule2) 

(*taga_l_rb_rula3) 

(*tage_l_rb_rula8> 

(*taga_l_rb_rulal2) 

(*taga_l_rb_rula7> 

(*taga_l_rb_rulalO) 

(*tago_l_rb_rula9) 

( » ta g e_l _r b _r u I *3 > 

(*taga_l_rb_rulal3) 

(»taga_l_rb_rulall ) 

(*taga_l_rb_rula6) 

(*taga_l_rb_rula4) ) ) ) 

(utar 

(simulation_run 

(rula (*taga_l_rb_rulal ) (*taga_l_rb_rula9> ) > > 
( ini t ial_valua 
(f rame_ful 1 

(rola (*taga_l_rb_rula2) 

(*taga_l_rb_rula3) 

(«taga_l_rb_rulaB) 

(«taga_l_rb_rulal2) 

(*taga_l_rb_rule7) 

(«taga_l_rb_rulalO) 

(«taga_l_rb_rula3) 

(•taga_I_rb_ruXaI3) 

(*taga_l_rb_ruiall> 

(*taga_l_rb ruX*4>))))> 

( »taga_X_rb_ruX«X (*tgpa (lfalX)) 

(craatad_bg (samir) > 

(eraatad_on (I4_4_89l)) 

(•if (tripia ( top_laval_choica top_laval_manu ) 

■ 

IDlsplag output* generated bg limulationl) 
(tripia (u»ar simulation_run) “ arror_fraa>) 

(•than (from 'output_di«pIag_choica> 

(loop 'out_di*p)) 

(mod if lad_bg (juan)) 

(nodif iad_on (I6_7JB31>)> 

(»taga_X_rb_rula2 (*tgpa (ifall)) 

(creatad bg (*amir>) 

(craatad_on (I2_20_B5I)> 

(•than (eval_aganda 'run_aganda> ) 

(•if (tripia (top_laval_choica top_laval_manu) 

m 

(Run simulation program!) 

(tripia ( inlt ial_valua frama_full) ■ trua>) 
(modlfiad_bg (juan)) 

(modif ied_on (I6_7_83m) 

(*taga_l_rb_rula3 (ftgpa (ifall)) 

(•if (tripia (top_leval_choica top_laval_manu ) 



- > IRun simulation program! > 

(•not (triple ( initial_value framo_full) “ truo)>) 

(created_by (samir)) 

( crsated_on (I4_29_83l>> 

(•than (c lause_pr int 

ITha simulation program cannot ba run uniats tha initial parameter !) 

(c lause_pr int 

lvalues needed for the simulation are given. You can use either!) 

(c laute_pr int (option 2 or 7 of the TOP LEVEL MENU. !>> 

(modif ied_by (I Juan J. Rodr iguez-Moscoso I ) ) 

(modif ied_on ( l&_tl_83l ) ) ) 

(stage_l_rb_rule8 (*type (ifall)) ” 

(created_by (I Juan J. Rodriguex-Moscoso I ) ) 

(created_on (I6_12_83l>> 

(•if (triple (top_levol_choice top_level_menu) 

I Set up initial parameter values l> 

(triple ( init lal_value frame_full> ■ true)) 

(•then (reset_rule_base 'user_rb) 

(back ' (user_rb_rulel ) 'uter_rb)) 

(modlfied_by (I Juan J. Rodriguex-Moscoso!)) 

(modif ied_on ( I6_12_83l ) ) ) 

(stage_l_rb_rulel2 (*type (ifall)) 

(created_by (tJuan J. Rodriguex-Moscoso! ) ) 

(created_on (!&_ll_83l)) 

(•if (triple (top_level_choiee top_level_menu > 

m 

ISet up initial parameter values!) 

(•not (triple ( ini tlal_value frame_full) ■ true))) 

(•then (eval_agenda 'sim_e>pert_agenda) 

(frem ' top_level_choice) ) 

(nodifled_by (I Juan J. Rodr iguez-Moscoso I ) ) 

(nodif ied_on ( I 6_12JB3! ) ) > 

(stage_l_rb_rule7 (*type (ifall)) 

(created_by (samir)) 

(created_on (12_23JB3I)> 

(•if (triple (top_level_choice top_level_menu) 

m 

IChange initial parameter values required for simulation! > 

(triple ( ini tial_value frame_full) “ true)) 

(•then (c lause_pr int 

lYou will see a menu of the parameters that can ba changed. By using!) 
(c lause_print 

(option 1 of this menu you can return to the TOP LEVEL MENU. I) 

(claut e_pr int > 

(c lause_pr int l«* BE CAREPULL **!) 

(clause_prlnt 

I - The values you enter will be passed on to the simulation program! > 
(clause_print I if you run it immediately.!) 

(clause_prlnt 

I - Thera is no intelligence present in this function. 8o> If you!) 
(clause_print 

I type in a wrong value an error might occur during the execution! > 
(clause_print I of the simulation program.!) 

(frem 'change_param_cholce) 

(loop 'change)) 

(modif ied_by ( I Juan J. Rodriguex-Moscoso & Bor-Jau Hsieh!)) 

(modif ied_on ( I6_13_85l ) ) ) 

(stage_l_rb_rulelO (*type (ifall)) 

(created_by (samir)) 

(created_on (I3_3_83!)) 



(*if (triple ( top_level_choice top_level_menu > 

m 

IChange initial parameter value* required for simulation!) 

(•not (triple ( initial_valua frama_full) ■ true))) 

(•than (clauae_print 

(There are no value* in the databaie. Pleate ute option 2 or 7 of I > 
(clau«e_print 

(the TOP LEVEL MENU to set up the parameter value*. They cannot bel) 
(clause_pr int (changed from this level. I)) 

(modif ied_by (I Juan J. Rodr iguez-Moscoso & Bor-Jau Hiiehl)) 

(mod if ied_on ( I6_13_83l ) ) > 

(*tage_l_rb_rule9 (*type (lfall)) 

(•if (triple (top_level_choica top_level_menu ) 

(Display outputs generated by simulation!) 

(•not (triple (user simulation_run) - error_f ree ) ) ) 

(created_by (samlr)) 

(cr«ated_on (I4_Z?_83I>> 

(•then (c lause_pr int 

(The output files generated by the simulation program cannot be dls-l) 
(c lause_pr int 

(played unless the FORTRAN program has run succesfully. If you have!) 
(clause_print 

(not done so< please use option 3 of the TOP LEVEL MENU to run the!) 
(c lause_pr int (simulation program.!)) 

(modlfled_by (IJuan J. Rodr iguez-Moscoso & Bor-Jau Hsleh!)) 

(modif ied_on ( I6_13_89l ) ) > 

(stage_l_rb_rule3 (*type (ifall)) 

(craated_by (samlr)) 

(created_on (12_20_83l)) 

(•if (triple (top_level_choice top_level_menu) 

(Display currant parameter values required for simulation!) 

(triple ( ini t ial_value frame_full) “ true)) 

(•then (c lause_pr int 

lYou will see a menu of the parameters which values can be seen by!) 
(clause_print 

Iselecting the appropiate choices. You can return to the TOP LEVEL!) 
(clause_prlnt (MENU by using option 1. !> 

(c lause_pr int ) 

(from 'disp_init_val_choice> 

(loop 'disp)> 

(modif ied_by (IJuan J. Rodr iguez-Moscoso & Bor-Jau Hsleh!)) 

(modif ied_on ( I6_13JB3I ) ) ) 

(stage_l_rb_rulel3 (Otype (ifall)) 

(•if (triple (top_level_choice top_level_menu) 

(Store current parameter values in a disk filet) 

(•not (triple ( lnltial_value frame_full> “ true))) 

(•then (clau*e_print 

(Thera are no values in the databa*e< hence no storage is possible!) 
(clause_print 

lat this point. You are suggested to use option 2 of the TOP LEVEL! > 
(clau*e_print MENU. )) 

(created_by (IJuan J. Rodr iguez-Moscoso ti Bor-Jau Hsleh!)) 

(created_on ( I6_13_83l ) ) ) 

(stage_l_rb_rulell (•type (ifall)) 

(created_bV (samir)) 

(created_on (I3_3 83!)) 



' * I Disp l«u current parameter value* required for simulation!) 

(•not (triple ( initial_value frame_full) - true))) 

(•then 

(clau»e_pr int 

(There are no values stored in the database) hence no display is possible. I)) 
(nod if ied_by (I Juan J. Rodr iguez-Moscoso & Bor-Jau Hsieh!)) 

(nodif ied_on ( I6_16_B3I > > ) 

(stage_l_rb_rul*6 (*type (ifall)) 

(created by (samir)) 

(created on (12 23 831)) 

(•if 

(triple (top_l*vel_choice top_level_menu ) 

m 

I Set up initial parameter values to default values! )> 

(•then (frem 'Y_jnatrix) 

(from 'quaternion) 

(frem 'response_chosen ) 

(frem 'control ler_type_des ired ) 

(frem ' inertial_jnat ) 

(frem 'Kd_matrix) 

(frem 'Kp matrix) 

(frem 'Kd> 

(frem 'lnltial_value> 

(frem 'inertial_matrix ) 

(frem 'Kp) 

(exec Is) 

(clause_print) 

(c lause_pr int 

IJust above is a listing of the current directory. Enter the name of the!) 
(cleu*e_pr int 

Iflle which contains the default values. Be sure the file exists in the!) 
(clause_prlnt 

(current directory) and it is a file created using option 8 of the TOP!) 
(clause_print 

■LEVEL MENU. If the file does not exist) then enter the name of any non!) 
(clause_print lexistlng file.!) 

(c lause_pr int ) 

(msg " » *•) 

(set_up_frames_from_f lie (read ) ) ) 

<modified_by (I Juan J. Rodr iguez-Moscoso & Bor-Jau Hsieh! )> 

(nodif ied.on < I6_16_83l ) ) ) 

(stage_l_rb_rule4 (*type (ifall)) 

(created_by (samir)) 

(created_on (12_23_83l)) 

(•if (triple (top_level_choice top_level_m*nu) 

IStore current parameter values in a disk filet) 

(triple ( initlal_value frame_full) - true)) 

(•then (exec Is) ** 

(clause_print) 

(c lause_pr int 

IJust above is a listing of all files in the current directory. Enter the!) 
(c lause_pr int 

■name of the file where the current parameter values will be stored. Bel) 
(c lause_pr int 

(sure the name you enter does not conflict with the existing files in the!) 
(c lause_pr int Ipresent directory.!) 

(clausejprint) 

(msg M » ••) 

(storage_of_inltial_values) ) 

(modlfied_by (I Juan J. Rodr iguez-Moscoso & Bor-Jau Hsieh! )> 
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(aodlf i»d_on U6_i6_83l>>> 

( chang a_param_c on trol 

(promp t_»p*c» (gat_altarnativa«_#rom ( param_m«nu ) ) 
(put_data_in (changa_param_choica) ) 
<data_lnput > ) > 

Cdiip_inlt_val_control 

<proi*pt_»p»c» (gat_altarnativa«_from (param_m»nu) ) 
(put_data_in < d iip_lni t_val_choic» > ) 
(ri.»»< nput) ) ) 



Cparam_menu (IRaturn to TOP LEVEL MENU!) 

CIReaponee typel) 

Cl A«i» of input command I ) 

CTO) 

CTfinal) 

CIDaltaT Ctima incramant)!) 

CIMultl-atep intagratlon arrorl) 

Cl Initial valuaa of Omega and Thetal) 

CIKp Matrix I ) 

CIKd Matrix!) 

Ctlnartlal Matrix!) 

CQuatarnion) 

C (Amplitude of input wave aignall) 

Cl Initial loueat frequency I ) 

C (Frequency deltaTI) 

C (Number of dacadeal) 

(I Number of aampllng frequenciea/decade I ) 
CIPhaaa of input wave aignall) 

C (Steady atate arrorl)) 



UlipJnitj/ilJjind* ((r«iit_ruli_t«ii 'dl«p_init_val_rb> ) 

((fram 'd lap_inl t_val_cholca > > 

( (manu_lnput 'di*p_tnit_val_controX ) ) 
((forward 'dl«p_intt_val_rb > ) ) 
<di*p_init_vai_rb (lf_ramovad (d_ramova_rula« ) ) 

(craatad_by (•amir)) 

. (craatad_on (I2J29JB3U) 


(rulas 


( par amt _ 
(di«p_ln 


(param_i»anu 


«p_init_va 
*p_init_va 
*p_lnlt_va 
«p_init_va 
•p_inlt_va 
*p_init_va 
«p_init_va 
*p_lnlt_va 
»p_init_va 
•p_init_va 
*p_init_va 
*p_tnit_va 
*p_lnit_va 
•p_init_va 
*p_lnlt_va 
»p_init_va 
*p_init_va 
ip_init_va 
•p_init_va 
n_if§ 
t_val_choice 


(ruli 


_rb_rulal > 

_rb_rola2) 

_rb_rula3) 

_rb_rula4) 

_rb_.ru la9) 

_rb_rul»6) 

_rb_rul«7) 

_rb_rula8> 

_rb_rol#9> 

_rb_rulalO) 

_rb_rul#ll ) 

_rb_rulal2> 

_rb_rulal3> 

_rb_rul#17> 

_rb_rul#18) 

_rb_rulal4> 

_rb_ru!alS) 

_rb_rulal6) 

_rb_rul#19> > 


di«p_init_val_rb_rulal) 

dl*p_init_val_rb_rula2) 

dl«p_init_val_rb_rula3) 

di*p_inlt_val_rb_rul#4) 



■<disp_init_val_rb_ruleS) 

(d isp_init_val_rb_rul#6) 

{ d i*p_ini t_val_rb_rule7) 

(disp_init_val_rb_rule8) 

(disp_init_val_rb_rule9) 

(disp_init_val_rb_rulelO) 

(d isp_init_val_rb_rulel 1 ) 

(disp_init_val_rb_rulel2> 

(d isp_init_val_rb_rulel3) 

(disp_init_val_rb_rulel7> 

(disp_init_val_rb_rulel8) 

(d i»p_init_val_rb_rol#14) 

(disp_init_val_rb_rulel5) 

(d lsp_init_val_rb_rulel6> 

(disp_init_val_rb_rulel9> > ) ) ) ) 
<di«p_init_val_rb_ruUl (*type (ifall)) 

<*if (tripla (di«p_init_val_ehoica param_menu> ■ TO)) 
(created_by (samlr)) ~ 

(creat»d_on (I2_23_83l>) 

(•than (dallm display) 

(♦print '( lnltial_valua TO))) 

(modif l#d_by (I Juan J. Rodr iguai-Moscoso I ) ) 

(modif ied_on ( W>_13_83l ) ) ) 

(disp_lnlt_val_rb_rul#2 (*typ# (ifall)) 

(•if 

(tripla (disp_lnit_val_chaica parara_manu>“ Tflnal)) 
(craatad_by (samlr)) 

(craatad_on (I2_23_83l)) 

(•than (daliin_display ) 

(fprlnt ' ( lnitlal_valua Tfinal))) 

(modif iad_by (I Juan J. Rodr iguai-Moscoso I ) ) 

(modif led_on < I6_13_83l ) ) ) 

(disp_init_val^rb_rula3 (*typa (ifall)) 

(craatad_.bg (samlr)) 

(craatad.on (12 23 831)) 

(•if 

(tripla (dlsp_init_val_choica pararo_manu) 

-IDaltaT (tlma lncramant ) I ) ) 

(•than (dalim_display ) 

(fprlnt ' ( ini t ial_valua daltaT))) 

(modif iad_by (I Juan J. Rodr iguai-Moscoso I ) ) 

(modif iad_on ( I6_13_B3I ) ) ) 

<(Ji *P_init_val_rb_rula4 (Styp# (ifall)) 

(craatad_by (samlr)) 

(creatad_on (12 23 831)) 

(•if 

(tripla (disp_init_val_choica param_manu) 

IMultl-stap lntagratlon arrorl)) 

(•than (dalim_display > 

(fprint ' ( initial_valua error))) 

(modified_by (IJuan J. Rodrlguez-Koscosol ) ) 

(modif ied_on ( I6_13_83l ) ) ) 

(disp_init_val_rb_rula3 (Stype (ifall)) 

(created_by (samlr)) 

(craatad_on (12 23 831)) 

(•if 

(tripla (disp_init_val_choice paramjnanu) 


Quaternion) ) 



(•than (dalim_diaplay ) (fprint '(quaternion) ) ) 
(modif ied_by (tJuan J. Rodr lguei-Koacoao I > > 
(modif led_on ( I6_13_83l ) > ) 

(dlf p_init_val_rb_rula6 (dtype (lfall)) 

(craatad_by (*an>ir)> 

(craatad_on (12 29 891 )) 

(•if 

(tripla (di*p_init_val_choica paraajnanu) 

I Initial valuaa of Omega and Thatal)) 
(•than (dalim_diiplay ) (fprint 'Y_matriK>> 
(modif iad_by (I Juan J. Rodr lguai-Moacoao I ) ) 
(modif iad_on ( I6_19_89l > ) ) 
(diap_init_val_rb_rula7 (*typa (lfall)) 

( craatad_by (aamir)) 

(craatad_on (12 29 891)1 
(•if 

(tripla (diep_inlt_val_cholco paramjnanu) 

I8taady atata arrorl)) 

(•than (dal im_d lap lay ) 

(fprint 

'( ini tial_value ataady_atate_arror ) ) ) 
(modif lad_by (I Juan J. Rodr iguax-Hoacoao I ) ) 
(modif iad_on (I6_19_89I))) 



<di*p_init_v«l_rb_rul«B (btype (ifall)) 

~ (created_by (•amir)) 

<cr»atad_on <12 27 8SIH 
(•if 

(tripla <diap_init_val_choice paraa_menu> 

IFnquincy ddtaTD) 

(•than ( del i*_di splay ) 

(terpri) 

(clause_print 

ITha inverse of frequncy daltaT ia displayed below. I) 
(fprint 

'( initial_valua frequency_deltaT) ) ) 

(atodif ied_by (wants_to_run_siaiif lation) ) 

(modif ied_on <I8_1_83I))> 



<<li*p_init_val_rb_rule9 (Ctype ( ifall >> 

(creatad_by (aarelr)) 

( cr eated_on (I2_27_83l)) 

(•if 

(triple (dl»p_init_val_cholce param_menu) 

(Amplitude of input wav* <ignall>) 
(•than (dali<n_diaplay ) 

(fprint '( initial_valua anplituda))) 

(mod if iad_by (IJuan J. Rodriguez-Moscoaol > ) 
(modif iad_on (16 13 831)) > 
(di*p_init_val_rb_rulelO (*typa (ifall)) 

(created_by (aamir)) 

(craatad.on (I2J27_83I>) 

(•if 

(triple (diip_lnlt_val_choice param_pianu) 

m 

(Initial lowest frequency))) 

(•than (dal im_diaplay ) 

(fprint 

'( lnltial_value init_f raquancy_valua ) ) ) 
(mod if ied_by (IJuan J. Rodr lguei-Moacoao I ) ) 
(modif ied_on (16 13 BSD)) 
(di«p_init_val_rb_rulall (*typa (ifall)) “ “ 



(■created_bg (samir)) 

(created_on (!2_27 851 )> 

(•if 

(triple (disp_init_val_choice param_menu) 


(Number of decadeel)) 

(•then (del im_d isp lay ) 

(fprint 

' ( initial_value number_of _decade» > ) > 
(modified_bg ((Juan J. Rodr lguet-Kos cosol ) ) 

(mod If ied_on ( I6_1S_85I > ) ) 

(di«p_init_val_rb_rulel2 (atgpe (ifall)) 

(created_.bg (eamlr)) 

(cr ea ted_on (I2_27 85U) 

( * i f 

(triple (disp_init_val_choice param_menu) 

m 

(Number of sampling frequencies/decade I > > 
(•then (delim_ditplag> 

(fprint 

' ( ini t ial_value 

number_of_sampl ing_frequencg_per_decade) ) > 
(modified_bg ( I Juan J. Rodr lguei-Moscoso I ) ) 

(mod if ied_on ( 16_15_85l ) ) ) 

(disp_init_val_rb_rulel3 (•tgpe (ifall)) 

(created_bg (samir) > 

(created_on (I2_27 850) 

(•if 

(triple (disp_init_val_cholce param_menu) 


(Phase of input wave signal!)) 

(•then (del im_disp lag ) 

(fprint '( ini t ial_value phase))) 
(modified_bg (I Juan J. Rodr iguei-Moscoso I ) ) 

(mod if ied_on ( I6_15_85l ) > ) 
(disp_init_val_rb_rulel7 (*tgpe (ifall)) 

(created_.bg (samir)) 

(created_on (IS 3 850) 

(•if 

(triple (disp_lnit_val_cholce param_menu) 

m 

(Amplitude of lmpulsel)) 

(•then (delim_displag) 

(fprint 

' ( ini t ial_value impulse_amplitude) > ) 
(modified_bg ( I Juan J. Rodr iguei-Moscoso I ) ) 

(mod if ied_on ( I6_1S_85I ) ) ) 
(disp_init_val_rb_rulel8 (*tgpe (ifall)) 

(created_.bg 

(I Juan J. Rodr iguez-Moscoso t< Bor-Jau Hslehl)) 
(created_on (I6_10 850) 

(•if 

(triple (dlsp_init_val_choice paramjnenu) 


(Response tgpe I ) ) 

(•then (delim_dlsplag) 

(fprint 

'(response_cho«en response_tgpe) ) ) 
(modified_bg ((Juan J. Rodriguez-Moscoso I ) ) 
(mod if ied_on ( I6_15_B5I ) ) ) 
(disp_init_val_rb_rulel4 (*tgpe (ifall)) 



• (Created_by (samir)) 

(created on ( I3_12 831 )> 

(•if 

(triple (disp_init_val_choice param_menu) 

■ 

IKp Matrix I ) ) 

(•then (del im_d itp lay ) 

(c lause_pr int ) 

(clauae_print 

(The Kp matrix it being displayed: I ) 

(clause _pr int 

) 

(printarray 'Kp> 

(terpri ) ) 

(mod if ied_by (IJuan J. Rodr iguez-Moscoso I ) ) 

(modif led_on ( I6_13_83l > ) > 

(disp_init_val_rb_rulel3 (Stype (ifall)) 

(created.by (samir)) 

(created_on (13 12 831)) 

(•if 

(triple (disp_lnit_val_cholce param_menu) 

■ 

IKd Matrix I ) ) 

(•then (dalim_dlsplay ) 

(clause_print > 

(clauta_pr int 

■The Kd matrix is being displayed:!) 
(clausejprint 

(printarray 'Kd) 

(terpri ) ) 

(modif led_by (IJuan J. Rodr iguez-Moscoso I ) ) 

(modif ied_on ( I6_13_83l ) ) > 

(disp_init_val_rb_rulel& (*type (ifall)) 

(created_by (samir)) 

(created_on (13 12JB3I)) 

(•if 

(triple (dlsp_inlt_val_cholce param_monu) 

llnertial Matrix))) 

(•then (delim_display ) 

(clause_pr int ) 

(clausejprint 

IThe Inertial matrix it being displayed:!) 
(clause_pr int 

> 

(printarray 'lnertial_matrlx ) 

(terpri)) 

(modified by (IJuan J. Rodrlguez-Moscoso I ) ) 

(modif ied_on ( I6_13_83l ) ) ) 

(disp_lnit_val_rb_rulel9 (ttype (ifall)) 

(•if 

(triple (disp_init_val_choice param _menu) 

■I 

I At is of input command!)) 

(created_by (samir)) 

(created_on (!&_13_B3I)) 

(•then (delim_display) 

( c lause_pr int ) 

( c lause_pr int 

■ Displaying the axis of the input command I ) 



% 






% 


» 


* 




n 


• (fprint 

' ( in it ial_value ax is_of_input_comraend > > ) 

(mod if i*d_by ( 1 Juan J. Rodr iguax-Moscoso I ) ) 

(modif i*d_on ( I 6_15_09l > > > 

< (r#s*t_rul*_ba*a 'chang*_param_rb ) ) 

((frem 'chang*_param_choic* > ) 

(tm»nu_input 'chang*_param_control ) ) 

((forward 'changa_param_rb ) > ) 

( IRcturn to TOP LEVEL MENU 1 ) 

((Plot of omagal) 

( (Plot of thatal ) 

( ICharac tar ist ics of tha stap raaponaa analysisl) 

(INumarical output ganaratad bg simulation I ) ) 
(output_displag_control (print 

ITha fimulation program ganarataa tha following output*: I > 
(print I - Option* 2 and 3 ara ASCII flla plotting*. I> 

(print I - Option* 4 and 9 ara numerical output*. I) 

(print 

IB* sura to hava run tha simulation bafora obsarving that) 
(print 

(output*. Tha outputs shown will corraspond to tha out-l) 
(print 

(put* ganaratad whan tha simulation was last run. I) 

(promp t_spacs ( gat_a 1 tarnat i vas_f rom 
(output_displag_m*nu) ) 

(put_data_in ( output_d isp lag_choica ) > 
(data_input ) > ) 

(output_displag_ag*nda ((fram 'output_displag_choica> ) 

( (ras*t_rul*_bas* 'output_di*p lag_rb ) > 

( (m*nu_input 'output _dl»plag_c on trol) ) 

((forward 'output_dlsplag_rb > > ) 

(output_d isp lag_rb (if_r#mov*d (d_r*mov*_rulas) ) 

(cr*atad_bg (samir)) 

(cr*atad_on (I4_4_S5I)> 

(rulas (output_dlsplag_rb_rula4) 

(output_di*plag_rb_rul*7) 

(output_d isp lag_rb_rulaB> 

(output_displag_rb_rul*9> 

(output_di»plag_rb_rul*10> 

(output_d i*p lag_rb_rul*ll ) 

(output_dlsplag_rb_rul*l ) 

(output_di«plag_rb_rula2> 

(output_d i»plag_rb_rul*5) 

(output_displag_rb_rul*6) 

( out put_d iip lay _rb_.ru 1*3) 

(output_displag_rb_rul*12) ) 

(param*_in_if s (output_d isp lag_choica 
(output_displag_manu 
(rula (output_displag_rb_rul*4> 
(output_displag_rb_rul*7> 

(output _di*plag_rb_rul *8) 

( output_d isp lag_rb_rul *9 ) 

( output_d isp lag_rb_r ul a 10) 

(output_d isp lag_rb_rul*l 1 ) 
(output_displag_rb_rul*l > 

(output_disp lag_rb_rul*2) 

(output_d isplag_rb_rul*9) 
(output_displag_rb_rul*6) 

(output_disp lag_rb_rul*3) 
(output_displag_rb_rul*12> ) > > 


( chang a_param_aganda 


(output_disp lag_manu 


(usar 



» > ( laet_raeponia_run 

(rule (output_d ltp lag_rb_rule4> 

(output_d ieplay_rb_rula7) 

(output _ditplag_rb_rule8> 

(output_d lap lay _rb_.ru la9) 
(output_diaplay_rb_rulalO) 
(output_diaplay_rb_rulal 1) 

(output_d ltplay_rb_rulal > 

(output_d lsp lay_rb_rule2) 
(output_diaplay_rb_rula3) 
(output_diaplay_rb_rula6) 
(output_ditplay_rb_rula3> 

( output_d ltp lag rb rulel2) > ) ) ) ) 

(output _diiplay_rb_rula4 (*tgp» (ifall)) 

(craatad_by (tamir)) 

(craatad_on (I4_4_83l)> 

(•than (ante cat numarOout. ttp ) ) 

<•1# (tripla ( output_d lip lag_cho 1 ca 
output_diaplay_manu) 

m 

INumtrlcal output ganaratad by almulatlonl ) 
(tripla (utar latt_ratponaa_run) 


IStap ratpontal)) 

(modlflad_.bg 

(I Juan J. Rodr iguaz-Motcoto Sc Bor-Jau Halahl)) 

(mod if lad_on (I6_9_83l))) 

(output_ditplay_rb_rula7 (ttgpa (ifall)) 

(•than (axac cat thataOcha. Imp ) ) 

(craatad_.bg damlr)) 

(craatad_on (I3_4_83D) 

(•If (tripla (output_dieplay_cholca 
output_diiplay_jnenu) 

m 

ICharac ter lit let of the analy*ltl> 

(tripla (utar laat_raaponta_run) 

m 

I Impulta ratponta I ) ) 

(modiflad_.bg 

(I Juan J. Rodrlguaz-Motcoto Sc Bor-Jau Halahl)) 

(modlf lad_on (I6_9JB3I))) 

(output_ditplag_rb_rulaB (ttgpa (ifall)) 

(•if (tripla (output_diaplay_choica 
output_dltplag_manu) 

INumarical output ganaratad by tlmulatlonl) 
(tripla (utar laat_raaponaa_run) 


(impulta ratponta))) 

(•than (eicec cat numerOout. imp ) ) 

(creatad_bg (tamir)) 

(created_on (I3_4_83D) 

(modif iad_bg 

(I Juan J. Rodriguez-Motcoto Sc Bor-Jau Htiahl)) 
(modiflad_on (I6_9_83l))) 
(output_ditplag_rb_rula9 (ttgpa (ifall)) 

(•than (axac cat numarOout. frq) ) 

(creatad_by (tamir)) 

(creatad_on (!3_4_85l)) 

(•if (tripla (output_dltplay_choica 
output_diiplay_manu) 



■Numerical output generated by simulation!) 
(triple (ueer laet_re<pon«e_run ) 

(Frequency reeponeel)) 

(modif ied_by 

('Juan J. Rodr iguez-Hoecoeo & Bor-Jau Heiehl)) 

(modif ied_on (I6_9_83l>)> 

(output_dieplay_rb_rulalO (ttype (ifall)) 

(•then (frequency_output_dleplay 'theta)) 

(created_by (samir)) 

(created_on (I3_4_83l)) 

(•if (triple (output_dleplay_choice 
output_dicplay_menu) 

(Plot of theta!) 

(triple (ueer laet_reepone e_run) 

■I 

IFrequency reeponeel)) 

(modif ied_by 

(I Juan J. Rodr iguez-Hoecoeo tc Bor-Jau Heiehl)) 

(modif ied_on (16 9 BSD)) 

(output_dieplay_rb_rulell (*type (ifall)) 

(•then (frequency_output_dieplay 'omega)) 

(created_by (eamir)) 

(creatad_on (I3_4_83l)) 

(•if (triple (output_dleplay_cholce 
output_dieplay_menu) 

■ 

(Plot of omega!) 

(triple (ueer laet_reeponee_run) 

(Frequency reeponeel)) 

(mod if ied_by 

(I Juan J. Rodr iguez-Hoecoeo l< Bor-Jau Heiehl)) 

(modif ied_on (16 9 831))) 

( output_dieplay_rb_.ru lei (»type (ifall)) 

(created_by (eamir)) 

(created_on (I4_4_83l)) 

(•if (triple (output_dleplay_choice 
output_dieplay_pienu) 

■i 

IPlot of ornegal ) 

(triple (ueer laet_reeponee_run) 

IStep reeponeel)) 

(•then (exec cat omegaOplt. etp) (c lear_diep lay > ) 

(modif ied_by 

( I Bor-Jau Heieh & Juan J. Rodr iguez-Hoecoeo I ) > 

(modif ied_on (!6_13 831))) 

(output_dleplay_rb_rule2 (*type (ifall) > 

(created_by (eamir)) 

(ereated_on (I4_4_B3D) 

(•if (triple (output_dieplay_cholce 
output_dieplay_menu) 

■I 

IPlot of thetal) 

(triple (ueer laet_reeponce_run) 

IStep reeponee I ) ) 

(•then (exec cat thetaOplt. etp ) (c lear_d iep lay ) > 



« ('modif ied_bg 

(IBor-Jau Hsieh & Juan J. Rodrlguex-Moscosol ) ) 

(modif ied_on ( I6_13_83l ) ) > 

( output _di»p lag_rb_rule3 (*tgpe (ifall >> ~ 

(created_bg (lamlr)) 

(craatad_on (I3_4_83l)> 

(•if (triple (output_displag_choice 
output_dlsplag_menu) 

■ 

IPlot of omagat ) 

(triple (user laet_raaponea_run) 

m 

I Impulse response!)) 

(•than (exec cat omegaOplt. imp ) (c lear_displag > > 

(modlfled_.bg 

(IBor-Jau Hsieh Si Juan J. Rodriguei-Moscoso t > > 

(modified on (16 13 831))) 

(output_d iip lag_rb_rula6 (•type (ifall)) 

(created_bg (samir)) 

(created_on (I3_4_B3I)) 

(•if (triple (output_displag_choice 
output_displag_menu> 

IPlot of thetal ) 

(triple (user last_response_run) 

(Impulse response))) 

(•then (exec cat thetaOp It. imp ) (clear_displag ) ) 

(modified_bg ~ 

(IBor-Jau Hsieh Si Juan J. Rodr lguex-Moscoso I ) ) 

(modif ied_on ( I6_13_83l ) ) ) 

(output_displag_rb_rule3 (»tgpe (ifall)) 

(created_.bg (samir)) 

(created_on (I4_4_a3l)) 

(•then (exec cat thetaOcha. stp ) ) 

(•if (triple (output_displag_cholce 
output_dlsplag_menu) 

(Characteristics of the step response analgsisl) 

(triple (user last_response_run) 

m 

IStep responsel)) 

(modif ied_bg (IBor-Jau Hsleht)) 

(modif ied_on ( I6_17_83l ) ) > 

(output _displag_rb_rulel2 (*tgpe (ifall) >~ 

(created_bg (u:ants_to_run_simulation ) ) 

(created_on (I3_3_83l).) 

(•then (c lause_prlnt 

(This option is for STEP response onlgi for FREQUENCY responsel) 
(clause_print 

(please use options 2> 3. and 3. I ) > 

(•if (triple (output_displag_choice 
output_displag_menu> 

m 

(Characteristics of the step response analgsisl) 

(triple (user last_response_run> 


IFrequencg response!)) 
(modif ied_bg (wants to_run_simulation) ) 
(modif ied_on ( I6_17J33I ) ) ) 

(run_rb (if_removed (d_remove_rules) ) 





(created_by (samir)) 

(created_on (I4_22_83l)) 

(rules (run_rb_rule2) (run_rb_rulel > ) 

(params_in_if s (respons e_ehosen 

(response_type (rule (run_rb_rule2> (run_rb_rulel > > ) ) 
(user (likes_Tf inal_value (rule (run_rb_rule2> ) > 

(Tf inal_yalue_f ound (rule (run_rb_rulel ) > > 
(wants_to_run_simulatlon (rule (run_rb_ruiel > ) ) > 
(initial_value (frame_full (rule (run_rb_rulel ) ) > > ) 
(param_specs (frame_full (f ind_strategy (fget>))> 

(param_conclusions (user (Tf inal_value_f ound (rule (run_rb_rule2> > ) ) ) ) 
(run_rb_rule2 (4type (lfall)) 

(creatad_by (samirt)) 

(created_on (I4_28_83l)> 

(•if (triple (response_chosen response_type ) “ IStep responsel) 
(prog nil 

(frem '(initlal_value TfinalH 
(fput ' < ini t ial_value Tfinal) 

(quotient 3.0 (one over tau 0>>) 

(f print '(lnltial_value Tfinal))) 

(triple (ueer like»_Tf inal_value) ■ get)) 

(•then (conclude (user Tf lnal_value_f ound ) true)) 

(•else (conclude (user Tf inal_value_f ound ) true) 

(frem ' ( ini t ial_value Tfinal)) 

(clause_print 

(Enter gour desired value for 'final time' (Tfinal): I > 
(msg N " » ") 

(fput ' ( ini t ial_value Tfinal) (read))) 

(modlf led_.bg (I Juan J. Rodr igue j-Moscoso ti Bor-Jau Hsiaht)) 
(modlf ied_on ( I6_13_B3I ) ) > 

(run_rb_rulel (*tgpe (lfall)) 

(craatad_by (saoiir)) 

(craated_on (I4_22_83l)> 

(•if (triple ( ini t ial_value frame_full) • true) 

(•or (*and (triple (response_chosen response_tgpe > 

m 

(Step responsel) 

(triple (user Tf lnal_value_found) » true)) 

(•and 

(triple (response_chosen response_tgpe) 


> (Frequency responsel)) 

(triple (response_chosen response_type ) 

i I Impulse responsel)) 

(triple (user uants_to_run_simulation) ■ yes)) 

(•then (clear_dlsplag) 

> (clause_print 

llnltial parameter values are being put into 'input file'!) 
(setup_lnit_val_in_simula. inp) 

> (c lause_print ) 

(clause_prlnt 

(The 'simulation program' is now being run. ..(Be patient)!) 
l (start_sim)> 

(modif ied_by ((Juan J. Rodriguez-Moscoso &c Bor-Jau Hsleht)) 
(modified_on ( I6_16_83l ) ) ) 

) (run_agenda ((frem 'eigr)> 

((frem 'eigl)) 

((frem 'one_over_tau) ) 

# ((frem 'user)) 

( (store_values_in_lisparray_from_frame 'Kp ) ) 


t 



((qa — uni, at eq~atna~qasaj) ) 
( (aiuaa# - u| — tan i » a — ua6 j a~ p»t>x ) ) 
, etusai - uioai - haaaadti i — uj — sanxrA - a aoqa ) j 
, aiuaaj - uioa*— -aacq*) ) 


< < s jaqcssf'iejqaauf 
((PM 



(user_rb (if_removed (d_remove_rules > ) 

(created_by (samir)) 

(created_on (I5_4_85l)) 

(param_specs ( dec is ion_about_r emoving_va lues (value_tgpe (literal) > 

(value_multlpllcitg 
(single) ) 

(valu»_requir«d (yes)) 
<prompt_f ormat 
<menu_input ) > 

( f ind_t trategg (ask)) 
(l«gal_valuti (gas) (no)) 


i ««< start back on tha left ««< 

(massaga_f orma t (print IDo gou wish to change tha 'Response tgpe' ?l) 

(print 

I - If gou sag YES< all the currant parameter values will be saved I > 

(print 

I and gou will be prompted for a new selection of 'Response tgpe'. 
(print 

I - If NQ< the 'Response tgpe' will be saved and all the currentl) 
(print I parameter values will be removed. I)) 
i »»> continue on the right >»» 

) 

(decision_about_removing_value) ) 

(rules (user_rb_rulel ) ) 

(params_in_if s 

(user (decision_about_removing_values (rule (user rb rulel)))))) 

(user_rb_ruiel (created_bg (samir)) 

(created_on (I3_4_B31)> 

(•else (raset_rule_base 'value_input_rb ) 

(frem '(user declsion_about_removlng_values) ) 

(frem 'response_chosen> 

(back ' (value_input_rb_rulel > 

'value_input rb)> 

(•tgpe ( ifal 1 > ) 

(•if (triple (user decision_about_removing_valuas) « no)) 

(•then (frem '(user decision_about_removlng_valuea> ) 

(eval_agenda 'sim_expart_agenda> ) 

(mod if i#d_bg ( 1 i kes_Tf Inal value)) 

(modif ied_on ( I6_13_83l ) ) )“ 



Simulation Model Listing 




c I Copyright (c) NASA Marshall Space Flight Center t 
c I Hunstville. Alabama I 
c I 19B5 I 
c 

c *««««****»»##«•«»»#»«*»*««*«•******«•***•*•*•««*•#***#**«*****•«•*«•*«* 
c * * 
c * Subroutine ANALYSIS * 
c * * 


c »***»••**«***#»*»*«****•*«•«*«*••***»•*••*»***»**»******«**•*•***•**••* 

c This soubroutine will analyse: 

c 1) step response: 

c Percent of Maximum Overshoot 

c Peak Time 

c . Rising Time <107. - 90%) 

c . Delag Time <0% - 50%) 

c . Settling Time (within SSE which was provided by the user) 

c 2) Frequency response: 

c when system in steady-state> the routine will calcualte 

c the summations of sine and cosine series. 

c 3) Impulse response: 

c Peak Vaule 

c Peak Time 

c Settling Time (within Steady State Error) 

c Written by Bor-Jau Hsieh (Andy) <Apr-03-B5) 

c Revised by Bor-Jau Hsieh (Andy) ti 

c Juan J. Rodrlguez-Moscoso (Jun-lZ-B3> 

SUBROUTINE ANALYSIS ( ndim. t. Y. Therr. Omerr ) 

c 

c* COMMON AREAS AND DIMENSION ARRAYS * 


IMPLICIT DOUBLE PRECISION (A-H.O-Z) 



COMMON 

/ 

FLAG 

/ 

FLAG ( 7 ) 






COMMON 

/READIN/ 

Bound ( 15) 

• 

Yin<10) 

a 

Kp (3. 3) 

+ 





Kd<3.3) 

a 

INMAT (3. 3) 

a 

INMATV<3. 3) 

+ 





ThcomO) 

1 

Omcom(3) 




COMMON 

/ 

STEP 

/ 

PM0(3) 

« 

RTini(3) 

a 

RTend (3) 

+ 





DT(3) 

» 

ST(3) 

a 

PT(3) 

4 * 





RT (3) 

i 

RTiminO) 

a 

RTemin(3) 

+ 





DTminO) 

• 

Thpeak (3) 




COMMON 

/ 

FRQ1 

/ 

Amp 

» 

Omega 

a 

Phase 

+ 

COMMON 

/ 

FRG2 

/ 

ts 

THAMP(3) 

a 

THFEEI3) 

a 

THAOL ( 3 ) 






THPOLO) 

a 

THS ( 3 ) 

a 

THC ( 3 ) 

+ 





XI (3) 

a 

X2(3) 

a 


+ 





OMAMPO) 

a 

0MFEE(3) 

a 

QMA0L(3) 

+ 





OMPOL(3) 

a 

OMS ( 3 ) 

a 

OMC (3) 






X3<3) 

a 

X4<3) 

a 

XN 


COMMON 

/ 

IMPU 

/ 

ST imp (3) 

a 

PT imp (3) 

a 

Thpeak_imp (3) 


COMMON 

/ 

PI 

/ 

PI 






COMMON 

/ 

DAMP 

/ 

damp_f lag 







DIMENSION 

Yt 10) 

i Therrt3) 

> 0snerr(3) 

INTEGER 

REAL*8 

LOGICAL*! 

damp_f lag 
Kp . Kd 

FLAG 

. INMAT . 

INMATV 




c# COMPUTATIAONS 




* 


STEP ANALYSIS 

IF ( FLAO(l) ) 

THEN 
I ° 3 

IF t FLAG 1 4 ) >1-1+4 

c *** Calculate Maximum Overmhoot (Thpeak) 

DO K - 1. 3 

IF ( DABS( Y(K+I> > . OT. Thpeak (K) ) 
THEN 

Thpeak t K I - Y(K+I> 

PTtK) - t 

END IF 
END DO 


c *** If no damping> only PMO b PT are computed. 

IF ( damp flag . EQ. 0 ) GOTO 9 
DO J- 1. 3 

c *** Calculate Rising Time (RT) 

IF ( DABS< DABS(Y< J+I ) ) - 0. ldO#Thcom(J) ) 

+ .LT. RTimint J) > 

THEN 

RTimin(J) - DABS( DABStYtJ+I>) - 0. ldO*Thcom<J) > 
RTini(J) - t 
END IF 

IF ( DABSt Y( J+I ) - 0. 9dO*Thcom( J) > 

+ .LT. RTemin(J) ) 

THEN 

RTemin(J) - DABS(Y(J+I> - 0. 9dO*Thcomt J) ) 

RTend(J) - t 
END IF 


c *** Calculate Delay Time (DT> 

IF t DABSt Y( J+I) - 0. 9dO»Thcom(J> > 

+ . LT. DTmin(J) > 

THEN 

DTmin(J) - DABSt Y( J+I) - 0. 5dO*Thcomt J) ) 
DTtJ) = t 

END IF 


c *** Calculate Settling Time 1ST) 

IF t YtJ+I) .NE. O. ODO ) THEN 
IF t DABStYt J+I ) - Thcomt J) ) 

+ . GT. tBound t6>*Thcomt J)/100. dO) ) 

+ THEN 

ST t J) = 0. ODO 
ELSE 

IF t STtJ) .EQ. 0. ODO ) STtJ) - t 



END IF, 

END IF 
END DO 
RETURN 
END IF 

c FREQUENCY ANALYSIS 

IF ( FLAC(2> > 

, + THEN 

IF I t . LT. ts ) GOTO 10 

t c *** Integrate SINE and COSINE Series 

THS(l) - THS( 1 ) + DSIN(Omega*t)*Therr< 1 > 

THS<2) - THS(2> + DSIN(Omega#t >*Therr (2> 

THS<3) - THS(3) + DSIN(Omega*t )*Thorr (3) 

THC(l) - THC(l) + DCOS(OmegaSt)*Therr(l> 

THC ( 2 ) - THC<2> + DCOS < Omeg a*t > *Th err ( 2 > 

THC (3) - THC(3) + DCOS(Omega*t >*Thorr (3) 

OMS(l) *= OMS< 1 ) + DSIN(Omega*t )*Omerr ( 1 ) 

0MS(2) - OMS ( 2 ) + DSIN(Omega»t)*Omerr (2) 

0MS(3) » 0MS<3> + DSIN(0mega*t)*0merr<3) 

OMC(l) - CMC ( 1 ) + DCOS(Omega*t )*Omerr ( 1) 

0MC(2) - OMC ( 2 ) ♦ DCOS<Omega*t ) *Omerr (2) 

OMC (3) *= OMC (3) + DCOS ( Ome g a* t ) »Omer r < 3 ) 

Compute the new Angular Position & Rate errors. and 
check Quaternion. 

CONTINUE 

IF < FLAO ( 4 ) > THEN 
prototype “1.0 
ELSE 

prototype = 0. 0 
END IF 

Therr(l) *» Therr(l) + Amp*DSIN(Omega»t+Phase)*Bound ( 13) 
Therr(2) “ Therr<2) + Amp«DSIN(Omegaet+Phase >«Bound < 14) 
Therr (3) “ Therr(3) ♦ Amp*DSIN(Omega*t+Phase )*Bound ( 15> 

Omerr(l) » Omerr(l) + Amp*Omega#DCOS<Omega»t+Phase)* 

+ Bound(13)e (1.0-prototype) 

Omerr(2) “ 0merr(2> + Amp*Omega*DCOS(Omega«t+Phase >* 

+ Bound(14)« (1.0-prototype) 

Omerr (3) » 0merr(3) + Amp*Omega*DCOS(Omega»t+Phase > * 

+ Bound(lS)* ( 1. O-prototype ) 

RETURN 
END IF 


c **» 

c 

* 10 

* 

■i 

i* 




IMPULSE ANALYSIS 

IF ( FLAG (3) ) THEN 

Checking out Quaternion Flag 
I - 3 

IF ( FLAG ( 4 ) ) I *» I + 4 
DO J - 1. 3 

Calculate Peak Value (Thpeak imp) 

IF ( Y(J+I) <3T. Thpeak_imp(J> ) THEN 
Thpeak_i mp ( J) = Y(J+I) 

PT_imp(J) = t 


c ess 


■c 


C 



I 


f 


aNB 
di onb 

NHfUBd 

oa onb 

31 ONB 
31 ONB 

* - (P) du»2 IS 

N3Hi (OP 0 03 - (P) <(uii is) di 

3SB3 

OP 0 = (P) du»x is 

N3H1 ( (OP OOT/(9)Punoa> 10 ' ((I+P)A)S3V ) 31 

(dial J.S) 3i“Ti 6uxx**es a*exn3X«?D ««* 3 
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# 



c I Copyright (c) NASA Marshall Space Flight Center 

c S Hunstville. Alabama 

c I 1985 


c ****»*»**«»»***««**»•*»«**••*««»««•«•«»«•«•••«*••*»«*»*«*«•»*«********» 
c * * 
c * SUBROUTINE BODY_DYNAM # 
c * * 
c *****#********#■*#*»*#************#****##****#*******#*#»******«»***#*** 


c #** Routine written by Juan J. Rodr iguez-Moscoso (Jan-24-B5) 

c Revised by Juan J. Rodr iguez-Moscoso & 

c Bor-Jau Hsieh (Andy) (May-10-85) 

SUBROUTINE BODY_DYNAM ( ndim, Torque, Y. Ydot ) 


c* COMMON AREAS AND DIMENSIONS ARRAYS 


w 

IMPLICIT DOUBLE 

PRECISION 

(A-H.O- 

Z) 




COMMON /READIN/ 

Bound ( 15) 

• 

Yin(lO) 

. Kp (3. 3) 

• 

+ 


Kd (3. 3) 

4 

INMAT (3. 

3) . INMATVO, 3) 

1 

+ 

COMMON / FLAO / 

ThcomO) 

FLA0(7) 

• 

0mcom(3) 




DIMENSION 

TorqueO) 

• 

. 

Torque command. 


+ 


Y( 10) 

• 

; 

state vector. 


+ 


Ydot ( 10) 

« 

i 

derivative of state 


♦ 


tmp(3)> tmpl(3) 

i 

Local storages. 



REAL*8 

Kp 

Kd 

INMAT . 

INMATV 



LOO I CAL# 1 

FLAG 






COMPUTATIONS 


c *** BODY DYNAMICS •»* 

DO i-1.3 

ia = i#(ll-3*i)/2-2 
ib = i*(3*i-13>/2+8 
tmp(i) - 0. OdO 
tmp 1 ( i ) « 0. OdO 

DO j*=l, 3 

tmp(i) *» tmp(i) + Y( j >*INMAT( ib. j ) 
tmp 1 ( i ) « tmp l(i> + Y< j ) *INMAT ( ia. j ) 
END DO 

tmp(i) ■ Y(ia)*tmp(i) 
tmpl(i) - Y < ib ) *tmp 1 ( i ) 
tmp(i) ■ Torque(i) - tmp(i) + tmpl(i) 

END DO 

DO i*l. 3 

Ydot ( i ) « 0. dO 
DO k = 1.3 

Ydot(i) = Ydot(i) + INMATVU. k)*tmp(k> 



& END DO 

END DO ' 

* c *** Quaternion check out. If the Quaternion block is present during 

c the SIMULATION, then RETURN and let the QUATERNION routine make 

c the computational work to forming the quaternion-rate eqs. 

IF ( FLA6 < 4 ) ) RETURN 

* Ydot ( 4) = Y< 1) 

Ydot ( 5) » Y ( 2) 

Ydoti 6) - Y( 3) 

RETURN 

END 


> 


> 


i> 

l> 


> 




Copyright <c) NASA Marshall Space Flight Center 
Hunstville. Alabama 
1985 


c *****■**#*#»«*********#*#*#********#**##*****#**#*####***#**#*#***##*»*# 
c * * 

c * Subroutine CHECK_ERRQR * 

c * * 

c ***#••#******»*»**»«#«»*•««»«•»**»****»**«**#*»•*•**#*•»**»»»»»***»*»»• 

c *** This subroutine check error flag '“ihlf" for both 
c Runge-Kutta and Predictor-Corrector integration methods, 

c 

c *** Written by Bor-Jau Hsieh (Andy) & 
c Juan J. Rodriguez-Moscoso (May-12-B5) 

SUBROUTINE CHECK ERROR ( ihlf, ICODE > 


COMMON AREAS AND DIMENSIONS ARRAYS 

IMPLICIT DOUBLE PRECISION (A-H.O-Z) 

COMMON / FLAG / FLAG (7) 

LOG I CAL* 1 FLAG 


COMPUTATIONS 


ICODE - 0 
ivar » ihlf - 10 
IF < ivar ) 50. 50, 10 

10 GO TO ( 20. 30. 40 ). ivar 

20 CONTINUE 

c *** Check FLAG ( i ) U-l.2.33 to determine OUTPUT unit 
DO i-1.3 

IF ( FLAG ( i ) ) 

THEN 

WRITE! i. 25) 

END IF 
END DO 
ICODE - 1 
RETURN 

30 CONTINUE 

c *«* Check FLAG ( i ) Ii-1.2.31 to determine OUTPUT unit 
DO i-1.3 

IF < FLAG ( i ) ) 

THEN 

WRITE! i. 35) 

END IF 
END DO 
ICODE » 1 
RETURN 


40 CONTINUE 

c *** Check FLAG(i) Ci=1.2.33 to determine OUTPUT unit 
DO i-1.3 


* 


IF ( FLAG ( i ) ) 
THEN' 

WRITE! i. 45) 
END IF 
END DO 
ICODE = 1 
RETURN 

50 RETURN 


c = 
25 


35 


45 


SSKBBaiSSeceasrSSCSSCSSS FORMATS BBSaBBBBBBIBKSeBSttnSSBBBBBSnKaBBBBBBBamt; 
FORMAT ( 1H0. 15X, 

+ 'BBBBBSBBBBBBaaBBBB ERROR DETECTED BBBBBBBBBBBBBBBBBBBB 1 

+ /1H . 15Xl 

+ ' There was no Convergence by applying this method of ' 

+ /1H , 15X. 

-I- ' Integration. Execution stopped. ') 

FORMAT ! 1H0. 15X. 

+ ' BSBSBSBBBBBBBBBBBB ERROR DETECTED BBBBBBBBBBBBBBBBBBBB < 

+ /1H . 15X. 

+ ' Doth, initial and final values of the interval under ‘ 

+ /1H . 15X, 

+ ' consideration, are zero. Execution stopped. ') 

FORMAT ( 1H0. 15X. 

+ <bbbbb»b...u.b... ERROR DETECTED 4 

+ /1H , 15X, 

+ 'The final value of the interval under consideration is ' 

+ /1H . 15X. 

+ 'less than the initial value. Execution Stopped. ') 


FORMAT ! 


FORMAT ( 


END 


i 

(; 


Copyright (c) NASA Marshall Space Flight Center 
Hunstville. Alabama 
1985 


c *#•»•*»»**»»*»#»«»»*•*#**••••»•*•••**»»*»•*•«•»*•«•««*••****«»#«*•*#**• 
c * * 

c * Subroutine CONTROLLER * 

c * # 

c *#*»#***#«**»»«***»•»»*«*#•»•*•*»••«**•**»••*•••**»»»«*****«#*#«*»»»**» 

c External subroutine to compute the right hand sides Ydot of the 

c system to given values x and Y. This routine, if accessed, 

c should not destroy x and Y. 

c 

c For this particular application, we consider Ydot as the 

c derivative of omega and theta in THE BOdy DyNamics equations. 

c •«* Routine written by Juan J. Rodr iguez-Moscoso (Jan-24-85) 

c Last revised by Juan J. Rodriguez-Moscoso 

c Bor-Jau Hsieh (Andy) (May-05-85) 

SUBROUTINE CONTROLLER ( ndim. Therr. Omerr. Torque ) 


COMMON AREAS DEFINITIONS AND ARRAYS 


IMPLICIT DOUBLE PRECISION (A-H. 0-Z) 


COMMON /READIN/ Bound (15) 
+ Kd (3, 3) 

+ ThcomO) 


Yin(lO) 
INMAT (3. 3) 
Qmcom(3) 


. Kp (3. 3) 

, INMATVO. 3) 


DIMENSION 

REAL«8 


Torque(3) 

Kp . Kd 


. Therr(3) . Omerr (3) 

. INMAT . INMATV 


c» COMPUTATIONS 


c *»* The torque dimensioning will be 3 C» ndim/23. Its computation is 
c done in this subroutine, but it can be modified to another rou- 

c tine which may be called from this onej i. e> the torque calcula- 

c tions will be enclosed in this block of programing. 

c •»* Also, the Input commands (desired output) is passed thru the 
c COMMON statement: 


COMMON /READIN/ Bound (15) 
Kd (3. 3) 
ThcomO) 


where. 


Yin(lO) 
INMATO. 3) 
OmcomO) 


Bound: Boundary conditions array. 
Yin: Vector of states. 

Kp: Matrix Kp(i.j) Controller. 
Kd: Matrix Kd(i.j) Controller. 
INMAT: Inertia Matrix, and 
INMATV: Inverse Inertia Matrix. 

Thcom: Position Command. 

Omcom: Angular Velocity Command. 


. Kp (3. 3) 

. INMATV (3. 3) 



I 



aN3 

NHrUBH 


oa aN 3 
□a aN 3 

(f)da«y)0*(r M)p* + 
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3 

3 



I 


c •»•«**•*»«•*•«•»•***»*««*»•»••*»•****»**«*«»#«*»***«**•«**•»*«##*«*«••• 
c * * 
c * SUBROUTINE DRAW_AXES * 
c * * 
c **•**•*#*«•*»*»«*#»**#*#**#•*«#*•*»»*•»•«*•»**»#•»*#*»#»***•#**#»*»#»»• 


c 

c 

c 

c 


Written by Juan J. Rodr iguez-Moscoso on 22-May-B5 
Last revised on 22-Jun-B5 


This routine draus the vertical and horizontal azes of the screen 
plot generated bg SCR_PLQTTER. 

SUBROUTINE DRAW_AXES ( iunit. Ymaz. Ymin, t. nt ) 

IMPLICIT DOUBLE PRECISION (A-H. 0-Z) 


COMMON / FLAG / FLAG (7) 

COMMON / SCRP / number_records> 
number _decadet. 
numb#r_samp_f 


DIMENSION 
INTEGER 
LOG I CAL* 1 
CHARACTER*S 
CHARACTER«4 
CHARACTER*3 
CHARACTER* 1 
CHARACTER*61 


tint) 

cero_pot 

FLAG 

zero _pos» 

upl 

home 

BAR. PLUS. 
AXIS 


moverlO 

MENOS. 


blank 


DATA BAR/'I '/. PLUS/'*'/. MENOS/'-'/. blank/' '/ 

call cursor_home( home. 1. idummy ) 
call cursor_up( upl. 1. idum. 1 ) 

WRITE! iunit. •) home 

IF ( Ymax .EG. 0.0 .AND. Ymin .EG. 0.0 ) 

THEN 

Ymaz ■ 1. O 
Ymin ■ -1. 0 
END IF 


WRITE! iunit. *) home 

call cursor_right! moverlO. 1. idummy. 10 ) 
do i°1.21 

Y = Ymaz - ! i-1 )*!Ymaz-Ymin)/20 

WRITE! iunit. 1010) Y. BAR 
end do 

IF I FLAG 12) ) ! Only for FREQUENCY Plotting. 

THEN 

DO i»l. number_decades-l 

muevete ” i*60/number_decades ♦ 10 

call cursor_r ight! zero _poi. 1. idummy. muevete ) 

WRITE! iuni t. *) home 

DO j-1.21 

WRITE! iunit. *) zero_pos. BAR 
END DO 
END DO 
END IF 



call cursor_rigl)t< iero_pos< 1> i dummy. 70 ) 
WRITE! iunit. *) home 
do 1 ^ 1,21 

WRITE! iunit. *) iero_pos, BAR 
•nd do 


IF < FLAG ( 1 ) ) 

THEN 
do i = l,6 

ii = 10*i-9 
AXISdi: ii) b PLUS 
do j = l,9 

ij *> ii + j 
AXIS(i j: i j) - MENOS 
■nd da 
end do 
ELSE 

DO J-i, 6i 

AXIS< J: J) » MENOS 
END DO 

AXISU: 1) = PLUS 

do j = l. number_decada*-l 

ntemp •• j»60/number_decadet + 1 
AXIS(ntemp: ntemp ) ■ PLUS 
■nd do 
END IF 

AXIS!61: 61 ) - PLUS 

WRITE! iuniti *) upl« moverlO. AXIS 


IF ( FLAG! 2) > 

THEN 

GO TO ( 10> 20, 30> 40. 50. 60 )> number decades 
10 WRITE! iunit. 1030) t. upl 

GO TO 70 

20 WRITE! iunit. 1040) t. upl 

GO TO 70 

30 WRITE! iunit, 1050) t. upl 

GO TO 70 

40 WRITE! iunit. 1060) t. upl 

GO TO 70 

50 WRITE! iunit. 1070) t. upl 

CO TO 70 

60 WRITE! iunit. 1020) t. upl 

GO TO 70 
ELSE 

WRITE! iunit. 1020) t, upl 
END IF 

70 CONTINUE 

IF ! Ymin . OT. O. ODO ) 

THEN 

cero_pos ■ 1 • 0 

ELSE 


cero « - 20*Ymin/ 1 Yma* - Ymin ) + 1. ODO 
c»ro_pos ” NINT! cero > 

END IF 

*ero_pos ! 1 : 5) ■ blank 

call cursor_up< iero_pos. 1. idummy. c»ro_pos ) 
IF ! cero_pos . NE. 0 ) 

THEN 



C=*e«o 

1010 

1020 

1030 

1040 

1050 

1060 

1070 


ipos = idurnmi) - 1 

WRITE( iunit. ») iero_pos < 1 : ipos ) , raoverlO. AXIS. upl 
END IF 

WRITE( iunit. *) home 

WRITE( iunit, *) moverlOi AXIS 

RETURN 


K3MSSSC3IS=«lsK=9Se=3 FORMATS a3aBCKBB3BanBaBB»BBSBa3BE333BaaBBBaC 
FORMAT < 1H . G9. 3. IX. A) 

FORMAT ( 1H > 7X> 7 1 G10. 2) > A) 

FORMAT ( 1H , 7X.C10. 2. 50X, CIO. 2. A) 

FORMAT ( 1H . 7X. 2(010. 2. 20X). 010. 2. A) 

FORMAT ( 1H . 7X. 3(010. 2. 10X ) . 010. 2. A) 

FORMAT ( 1H . 7X< 4(010. 2. 5X). 010. 2. A) 

FORMAT ( 1H . 7X< 5(010. 2,2X1.010. 2.A1 
END 
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c It********************************************************************** 
c * * 

c * SUBROUTINE EIGEN * 

c * * 

c *••«»••*»»*•**•«•«****»«•»»»»#•*•»*•*»*•*»#*•«##»»»«***»***»»***•*#»#•* 

c *** Subroutine written bg Juan J. Rodr iguez-Moscoso (Apr-lB-85) 

c Revised on Apr-19-QS ! Includding computation of A matrix, 

c inhere the A matrix is fixed bg Kp. 

c Kd. and the Inertia matrices. Also, 

c it is given that INMAT “ aeldentitg. 

c on Mag-09-85 ! Commenting out first approach of the 

c eigenvalues computation. 

SUBROUTINE EIGEN! KP. KD, INMAT. TAU. EIGR, EIGI, I ERR ) 


c *** 

c 

c 


If I ERR = 
I ERR ! 


then Eigenvalues Computation is successful, 
then an ERROR has been produced and no sol- 
ution has been reached. 


ARRAY DIMENSIONING 


IMPLICIT DOUBLE PRECISION (A-H.O-Z) 

REAL«B KP. KD. INMAT. INMATV 

DIMENSION KP(3.3). KD(3.3>. INMAT(3.3>. EIGRI6). EIGI (6) > 

A<6, 6). INMATV (3, 3) > INT!&>. SCALE (6) 

DATA A/3A«0. DO/ 

c *** Finding the inverse of the Inertia matrix 

DO 5 1 = 1,3 
DO 4 J*l» 3 

INMATV! I. J) = INMAT(I.J) 

CONTINUE 
CONTINUE 

CALL MATINV! INMATV. 3. ICODE ) 

c *** If ICODE is not xero ==> The Inertia matrix is singular. 

IF! ICODE .NE. 0 ) THEN 

I ERR = -1 ! I ERR = -1 No inverse 

RETURN 

END IF 



Identitg 


0 



DO 20 1 = 1,. 3 
DO 10 J=l. 3 

A(I. J) = 0. DO 
DO 10 K=l. 3 

A( I. J) = Ad. J)-INMATVd.K)*KD(K, J) 

10 CONTINUE 

DO 20 J=l,3 
JI = J+3 
DO 20 K=l. 3 

Ad. JI ) = Ad. JI )-INMATV(I. K)*KP (K. J) 

20 CONTINUE 

DO 30 1=4.6 

Ad. 1-3) = 1. DO 
30 CONTINUE 

c *** Computing the Eigenvalues of A 

CALL BALANC ( 6. A. A. LOU. IOH. SCALE > 

CALL ELMHESI 6. 6. LOU. IGH. A. INT ) 

CALL HGR ( 6. 6. LOU. ICH. A. EIGR. EIOI. I ERR ) 

c *** End of Eigenvalues Computation 


c *** 
c 


40 


c * 
C * 
c * 


Computing the maximum eigenvalue in module. Ue assume that 
real parts of the eigenvalues are less than zero. 


DABS( EIGR (I) ) 


TAU - DABS( ElGRd ) ) 

DO 40 1=2.6 

IF ( TAU .LT. DABS(EIGRd) ) ) TAU 
CONTINUE 

RETURN 

END 

•««#*«*•*«*«*##«•«**•«***••***•••*•»»*•****««»***««*****»«*•#**»**••*»* 

* 
* 
« 


SUBROUTINE BALANC 

C *********************************************************************** 

c *** Routine written by Juan J. Rodr iguez-Moscoso (Apr-3-8S) 

c Revised bg Juan J. Rodriguez-Moscoso (Apr-18-85) 

SUBROUTINE BALANC ( NM. N. A. LOU, IGH. SCALE ) 

IMPLICIT DOUBLE PRECISION (A-H.0-2) 

DIMENSION A(NM.N). SCALE(N) 

LOGICAL NOCONV 

c ••• RADIX specifies the Base of the machine floating point representation 
RADIX = 2. DO 

B2 = RADIXsRADIX 
K = 1 
L = N 
GOTO 100 

c **♦ Exchanging rou> and column 

20 SCALE(M) ** J 


/ 



IFC J . NE., M ) THEN 
DO 30 1*1, L 

F = A(I.J) 

ACI.J) * ACI.M) 

ACI.M) «* F 

30 CONTINUE 

DO 40 I*K, N 

F * A(J, I) 

A< J, I) •= ACM. I) 

ACM, I) * F 

40 CONTINUE 

END IF 

GOTOC 80.130). IEXC 

c *** Searching for rows isolating an eigenvalue and 

c push them down 

80 IFC L EQ. 1 ) THEN 

LOW - K 
IGH * L 
RETURN 
END IF 

L - L-l 

100 DO 120 J*L. 1. -1 

DO 110 1*1. L 

IFC I .NE. J ) THEN 

IFC AC J. I ) .NE. 0. DO ) GOTO 120 
END IF 

110 CONTINUE 

M * L 
IEXC = 1 
GOTO 20 

120 CONTINUE 

GOTO 140 

130 K = K+l 

140 DO 170 J*K< L 

DO ISO I=K. L 

IFC I .NE. J ) THEN 

IFC AC I, J) .NE. 0. DO ) GOTO 170 
END IF 


ISO 

CONTINUE 


170 

IEXC * 2 
M = K 
GOTO 20 
CONTINUE 


c *** 

Balancing the 

submatrli in rows K to L 

180 

DO 180 I*=K. L 

SCALE C I ) » 

1. DO 

c *** 

Iteration for 

Norm Reduction 

190 

NOCONV = . FALSE. 



DO 270 I=K. L 
C = Of. DO 
R “ 0. DO 


DO 200 J=K. L 

IF ( J .NE. I ) THEN 

C = C+DABS( A( Ji I) ) 

R - R 4-DABS ( A(l.J) ) 

END IF 

200 CONTINUE 

0 - R/RADIX 
F “ 1. DO 
S - C 4- R 

210 IF ! C .LT. G > THEN 

F = F#RADIX 
C = C*B2 
GOTO 210 
END IF 
G = ReRADIX 

230 IF C C .GE. C ) THEN 

F - F/RADIX 
C = C/B2 
GOTO 230 
END IF 

C *** Balancing 

IF < (C4-R1/F .LT. 0. 95D0»S ) THEN 
SCALE! I) - SCALE! I )*F 
G = 1. DO/F 
NOCONV - . TRUE. 

DO 250 J=K» N 

A! I> J) * All. J)*0 
250 CONTINUE 

DO 260 J-l.L 

A! J. I) - A! J. I )*F 
260 CONTINUE 

END IF 

270 CONTINUE 

IF! NOCONV ) GOTO 190 

LOW = K 

IGH - L 

RETURN 

END 

c a*********************************************************************** 
c * * 

c * SUBROUTINE ELMHEB * 

c * * 


SUBROUTINE ELHHES! NM. N. LOW. IGH. A. INT > 

c **# Routine written bg J. Rodr iguet-Moscoso IApr-03-85) 
c Last Revision: Apr-18-85 !JR-M> 



I 

) 

; 

> 

J 

' 100 

c **» 

HO 

120 
C **» 


140 

150 

160 

1B0 


IMPLICIt'dOUBLE PRECISION (A-H.O-Z) 

DIMENSION A(NM.N). INT(IGH) 

LA = IGH-1 
KP1 ■ LOM+1 

IF < LA .LT. KP1 ) RETURN 

DO 1B0 M=KP 1 1 LA 
MM1 - M-l 
X “ O. DO 
I ** M 

DO 100 J=*M« ICH 

IF < DABS(A(J. MM1)) . GT. DABS(X) ) THEN 
X - A(J.MMl) 

I =» J 
END IF 
CONTINUE 

INT(M) « I 

IF( I .NE. M ) THEN 

Intarchanging Rous and Columns of A eeeeeee**** 
DO 110 J=MM1. N 

Y = A( Ii J) 

A(I. J) = A CM. J) 

AIM. U) = Y 

CONTINUE 
DO 120 J=l. IGH 

Y - A(J. I) 

A(J. I) = A(J.M) 

A(J. M) - Y 

CONTINUE 
END IF 

End of the Interchange *#####**#**##***»####*# 

IF( X .NE. 0. DO ) THEN 
MP1 - M+l 
DO 160 I“MP1. ICH 
Y “ A(I.MMl) 

IF ( Y .NE. 0. DO > THEN 
Y = Y/X 
A(I.MMl) «* Y 
DO 140 J=M.N 

A( I. J) - AII. J)-Y*AIM. J) 

CONTINUE 
DO 150 J=1,ICH 

A(J.M) « A ( J. M ) +Y* A ( J. I ) 

CONTINUE 
END IF 

CONTINUE 
END IF 
CONTINUE 

RETURN 

END 


c «**»**»«##»*******»*»»#******»****»**»»»#***»»#»***•*»#*»»*»«»»*#»*»#* 
c * # 



SUBROUTINE HQR 


c * 
c * 


c ***#**#***####***#***■#*#*********##**#**»**»****#**#**»» **.»***»«•»*#»**«. 


c *** Routine written by J. Rodr igue x-Moscoso (Apr-02-B5) 

c Revised by J. Rodriguei-Moscoso (Apr-18-B5) 

SUBROUTINE HQR t NM. N, LOW. IGH. H. WR, MI. IERR ) 

IMPLICIT DOUBLE PRECISION (A-H. O-Z) 

REAL*8 MACHEP 

INTEGER EN. ENM2 

DIMENSION H(NM. N). WR(N>, UI(N> 

LOGICAL NOTLAS 

C as* MACHEP specifies precision of floating point arithmetic 
C MACHEP - I. DO/2. D0**55 

MACHEP - . 2BD-16 
IERR - 0 

C see Staring Roots isolated by BALANC 
DO 50 I«1,N 

IF( I .LT. LOW .OR. I .GT. IGH ) THEN 
UR(I> - H(I. I) 

UI( I) - 0. DO 

END IF 

50 CONTINUE 

EN = IGH 
T = 0. DO 

C *ee Searching for next Eigenvalues 

60 IF ( EN .LT. LOU ) RETURN 

ITS - 0 
NA = EN-1 
ENM2 » NA-1 

C e*e Looking for single small sub-diagonal element 

70 DO BO L = EN.LOU, -1 

IF ( L . EQ. LOU > GOTO 100 

RINTER = MAC HEPS ( DABS<HIL-1. L-l > > + DABS(H(L, L) ) ) 
IF< DABS(H(L. L-l) ) . LE. RINTER > GOTO 100 
BO CONTINUE 


C ee* Forming shift 


100 X - H(EN. EN) 

■> IF( L .EQ EN ) GOTO 270 

Y = H(NA.NA) 

U « H(EN.NA)eH(NA. EN) 

> IF< L -EQ. NA ) GOTO 2B0 


J 


IF ( ITS .EQ. 30 ) THEN 
IERR » EN 
RETURN 


J 


J 


ELSE 



IF < ITS . NE. 10 . AND. 

* * ITS .NE. 20 ) GOTO 130 
T - T+X 

DO 120 I=LOW, EN 

HI I> I) = HII. I)-X 
, 120 CONTINUE 

S = DADS ( H ( EN. NA ) ) +DABS (HINA. ENM2 ) ) 

X * 0. 75D0*S 
Y = X 

W = -0. 4375D0*S*S 
END IF 

130 ITS = ITSU 

C Looking for two consecutive smell sub-diagonal elements 

DO 140 M = ENM2. L. -1 
ZZ = HIM. M) 

R = X-ZZ 
B ■= Y-ZZ 

P = 1 R*S-W )/HIM+l. M)+HIM> M+l ) 

Q «= HIM+1. M+l J-ZZ-R-S 
R = HIM+2. M+-1 > 

S =■ DABS I P ) +DABS I Q ) 4 DABS I R ) 

P = P/S 
Q > Q/S 
R = R/S 

IF! M .EO. L ) GOTO 150 

IF! DABS I HIM. M-l ) )*l DABS ( Q ) +DABS I R ) ) . LE. 

* * MACHEP *DAB S I P > * I DABSIHIM-1. M-l ) )+DABSIZZ>+ 

* DABSIHIM+1. M+l ) ) ) > GOTO 150 

140 CONTINUE 

150 MP2 = M+2 

DO 160 I=MP2. EN 

* HII. 1-2) = 0. DO 

IF< I .NE. MP2 ) HII. 1-3) - 0. DO 
160 CONTINUE 

C *** Doubling OR step involving rows L to EN and columns M to EN 

DO 260 K=M.NA 

NOTLAS = . FALSE. 

IF! K .NE. NA ) NOTLAS - .TRUE. 

IFI K . NE. M > THEN 
P - HIK.K-1) 

Q « HlK+l.K-1) 

R - O. DO 

IF! NOTLAS ) R - HIK+2.K-1) 

X = DABSIP)+DABSIQ)+DABSIR) 

* IFI X .EQ. 0. DO) GOTO 260 

P - P/X 

Q = Q/X 

» R *= R/X 

END IF 

► C S - SIGN! DSQRT(P«P+Q*0+R*R).P ) 

c > The next change has been introduced in order to get 

c a compiled version of the routine under EUNICE F77 




c 


compiler < 


S = DSGRT< P*P+Q#Q+R*R ) 

IF ( P . LT. 0. DO ) S = -l.DO*S 

IF ( K . NE. M ) THEN 
H(K. K-l ) = -S*X 
ELSE 

IF ( L .NE. M > H(K> K-l ) *■ -H(K. K-l) 
END IF 

P = P+S 
X = P/S 
Y - Q/S 
ZZ » R/B 
Q a Q/P 
R = R/P 

C *** Row Modification 

DO 210 J=K.EN 

P = H(K. J)+Q*H<K+1. J) 

IF( NOTLAS ) THEN 

P » P+R«H(K+2.J> 

HIK+2.J) “ H(K+2> J)-P*ZZ 
END IF 

HtK+l.J) = H<K+1.J)-P#Y 
H(K. J) ■ H(Kt J)-P*X 
210 CONTINUE 

J - MINO( EN.K+3 ) 

C *** Column Modification 

DO 230 I=L. J 

P = X*H( I» K)+Y*H( I« K+l ) 

IF ( NOTLAS ) THEN 

P =« P+ZZ*H< I > K+2) 

HII.K+2) - Hd,K+2)-P*R 
END IF 

Hd« K+l ) *= Hd# K+l )-P*Q 
H(I.K> - H( I. K)-P 


230 

CONTINUE 

260 

CONTINUE 
GOTO 70 


C *** 

One root 

found 

270 

UR(EN) - 

X+T 


WI(EN) ° 

0. DO 


EN - 

NA 


GOTO 60 



C *** Two roots found 

280 P - < Y-X )/2. DO 

Q » p»p+W 

ZZ ■= DSQRT ( DABS(Q) ) 
X = X+T 




IF ( Q . GE. 0. DO, ) THEN 
* * 


C ##* Real Pair 

ZZ * P+SIGN< ZZ.P ) 

This change is introduced in order to get a compiled 
version of the routine under EUNICE F77 compiler < — 
IF ( P .LT. 0. DO > ZZ - P+ZZ 

IF ( P .GE. 0. DO ) ZZ =» P-ZZ 

WR(NA) - X+ZZ 
WR(EN) *= WR(NA) 

IF t ZZ .NE. 0. DO ) WR(EN) « X-W/ZZ 
UKNA) = 0. DO 
WI(EN) *■ O. DO 

ELSE 

C *** Complex Pair 

NR(NA) « X+P 
WR(EN) - X+P 
UKNA) «= ZZ 
UI (EN) » -ZZ 

END IF 

EN - ENM2 
GOTO 60 

END 






DO 100 t*=tO. tf. del tat 

CALL SYSJDYNAM ( ndim. t. Y. Ydot ) 

CALL 0UTPUT_17 < ndim. t. diltat. Y. Ydot ) 
DO i=l i nd im 

Y(i) = Y< i ) + deltat*Ydot ( i ) 

END DO 

100 CONTINUE 

RETURN 

END 


* 




Copyright * (c ) NASA Marshall Space Flight Center 
Hunstville. Alabama 
1985 


c **#«**#****««*««*»*«•**•**•**«*«**«««***«***•*******««#**••******»«•**• 
c * * 
c * Subroutines for Graphic Purposes • 
c * * 
c *«*«•##»*****»»**»»»•*»»»•**••*••*****•«•*»**»*«»•**»•*»»•**«*»•»***•»* 

c *** Written by Juan J. Rodr iguez-Moscoso (Apr-21-85) 


Revised on <May-05-85) 


Revised on (May— 07-85) 
Revised on <May-10-B5> 
on May-13-B5 


SUBROUTINES: 


! Adding unit as an argument to each 
! routine in order to use them on file 
! writing. 

! Including comments for each subroutine. 

! and addition of new subroutines 
! Including new subroutines and 
modification of the main routines. 

! Adding type of format (formatted or un- 
formatted) on the writing routines. 

PURPOSE: 


- Enter_graph_mode ( iunit ) 

- Exit_graph_mode( iunit ) 

- Enter_hold_* c reen_made( i w ) 

- Exit_h°ld_* cre * n _ n,0, let iu ) 

- Clear_display ( iunit. ans ) 

- Transmit_page( iunit. ans ) 

- Transmit_curr_line( iunit ) 


- Transmit_char_at_cursor ( iu ) 


- Transmit_25th_line( iu ) 

- Cursor_up( str. is. ie. n ) 


- Cursor_down( str. is. ie. n ) 


Cursor_right( s. is. ie. n ) 


- Cursor_left< s. is. ie. n ) 

- Cursor_home( iunit ) 

- Enter_al t_char_set ( iunit ) 

- Exit_alt_char_set( iunit ) 


This routine sets graphic mode. 
This routine resets normal 
mode of operation. 

This routine sets the terminal 
to be hold. 

This routine resets the termi- 
nal to normal mode. 

Clears screen. 

Transmits new page to the out- 
put screen or unit 'iunit'. 
Transmits the current line of 
output to the screen (iunit»6) 
or unit 'iunit'. 

Transmits the current character 
at the cursor to output screen 
on unit 'iu'. 

Transmits the 25th line to out- 
put screen on unit 'iu'. 

Moves the cursor forward n pos- 
itions. Results are written in 
string 'str'. 

Moves the cursor backwards n 
positions and result is stored 
in string 'str' 

Moves the cursor to the right 
n positions and result is 
stored in string 's'. 

Moves the cursor to the left 
n positions. 

Moves the cursor to home posit. 
Sets the alternate character 
set of instructions. 

Exits the alternate char. set. 



- Enter_alt_char_qraph_set( iu ) 

- Es i t_a 1 t_char_graph_set ( iu ) 

- Cursor_off( iunit ) 

- Cursor_on( iunit ) 

- Save_cursor_position( iunit ) 

- Cursor_to_save_position( iu ) 


Enter the alternate character 
graphic set of instructions. 
Disables the action of the 
above routine. 

Turns off the cursor. 

Turns on the cursor. 

Saves the current cursor pos- 
ition. 

Sets the cursor to the previous 
ly saved position. 


c *** WARNING *** 

c > Do not modify these routines without 

c talking to Juan J. Rodr iguez-Moscoso <■ 


subroutine enter_graph_mode ( el_codigo< is. ie ) ! (May-10) 

c <ESC>[ 10m 

character*!#) el_codigo 

el_cod igo( is: is+4) *> char(27)//char(9l )//char(49)//char(48) 

+ //char < 109) 

ie *= is + 3 
return 
end 
c 

subroutine exit_graph mode ( el codigo. is. ie ) ! (May-10) 

c <ESC>C 11m 

character*!*) el_codigo 

el_codigo( is: is+4) - char (27>//char (91 )//char(49)//char (49) 

+ //char ( 109) 

ie ■ is + 5 
return 
end 
c 

subroutine enter hold screen mode ( el codigo. is. ie ) 

c <ESC>C>3h 

character*!*) el_codigo 

el_codigo(is: is+4) - char (27)//char (91 )//char (62) //char ( 51 ) 

+ //char ( 104) 

ie a is + 5 
return 
end 
c 

subroutine exit_hold_screen mode ( el codigo. is. ie ) 

c <ESC>C>31 

character*!*) el_codigo 

el_codigo(is: is+4) - char (27>//char (91 )//char (62)//char (51 ) 

+ //char ( 108) 

ie = is + 5 
return 
end 


subroutine clear_display ( iunit. ans ) ! (Adding unit number) 

c <ESC>C2J<ESC>1H 

character*! ans 

character*7 el_codigo 

el_codigo( 1: 3) = char(27)//char (91 )//char (72) 
el_cod igo(4: 7) = char (27>//char (91 )//char (50)//char(74) 
if ( ans . eq. ) then 

write( iunit. *) el_codigo 
else 


« 

1 

i 



end i f 
return 
end 

subroutine transmit_page ( iunit, ans ) ! (Adding unit number) 
<ESC>tp 

characters el_codigo 

character*! ans 

el_codigo = char (27) //char (91 ) //char ( 1 12) 
if ( ans . eq. '*' ) then 
write (iuniti *) el_codigo 
else 

write(iunit) el_codigo 
end if 
return 
end 

subroutine transmit curr line ( iunit ) ! (Adding unit number) 
<ESC>Clp 

character#4 el_codigo 

el_codigo ■ char(27)//char(91)//char(49)//char(112) 

write(iunit) el_codigo 

return 

end 

subroutine transmit_char at cursor ( iunit > ! (unit number) 

<ESC>12p 

character*4 el_codigo 

el_codigo*char (27) //char (91 )//char (50) //char (112) 

write(iunit) el_codigo 

return 

end 

subroutine transmit_25th line ( iunit ) ! (Adding unit number) 
<ESC>C3p 

character*10 el_codigo 

el_codigo=char(27)//char(91)//char(51)//char(112) 

write(iunit) el_codigo 

return 

end 

subroutine cursor_up ( el codigo, is, ie, n ) ! (Mag-10) 

<ESC>CPnA 

character#!*) el_codigo 
nsave “ n 
icounter •» 0 
(counter = icounter + 1 
if ( nsave . ge. 100 ) then 
nsave “ nsave/10 
goto 1 
end if 

icoc “ nsave/10 
imod « mod (nsave, 10) 

el_codigo( is: is+1 ) = char (27)//char (91 ) 
if( icoc . eq. O ) then 

el_codigo( is+2: is+3) => char (nsave+4B) //char (65) 

ie - is + 4 

return 


else 




el_codigo< is+2: is+2) = char (4B+icoc ) 
el_codigo( is+3:’is+4) “ char (48+imod)//char(65) 
ie = is + 5 
return 
end if 
end 

subroutine cursor_doun ( el codigo, is, ie, n ) ! (Mag-10) 

<ESC>tPnB 

character*!*) el_codigo 
nsave ■> n 
icounter » 0 
icounter •» icounter + 1 
if ( nsave . ge. 100 ) then 
nsave «* nsave/10 
goto 1 
end if 

icoc — nsave/10 
imod = mod (nsave, 10) 

el_cod igo( is: is+1 > ■ char (27)//char(91 ) 
if ( icoc . eq. 0 ) then 

el_codigo(is+2: is+3) ** char (ntave+48) //char (66) 

ie = is + 4 

return 

else 

el_codigo(is+2: is+2) ■ char (48+icoc ) 
el_codigo( is+3: is+4) ** char (4B+imod )//char (66) 
ie ■ is + 5 
return 
end if 
end 

subroutine cursor right < el codigo, is, ie, n ) ! (Mag-10) 

<ESC>CPnC 

charactere(e) el_codigo 
nsave ” n 
icounter *■ 0 
icounter ” icounter + 1 
if ( nsave . ge. 100 ) then 
nsave •• nsave/10 
goto 1 
end if 

icoc • nsave/10 
imod <= mod (nsave, 10) 

el_cod igo( is: is+1 ) « char (27)//char (91 ) 
if ( icoc . eq. O ) then 

el_codigo(is+2: is+3) ** char (48+nsave ) //char (67 > 

ie = is + 4 

return 

else 

el_codigo( is+2: is+2) «• char (4B+icoc ) 
el_codigo( is+3: is+4) « char (48+imod ) //char (67) 
ie = is + 5 
return 
end if 
end 

subroutine cursor_left ( el_codigo, is, ie, n ) ! (Mag-10) 

<ESC>CPnD 

character*!*) el_codigo 


i 


V, 

(. 

* 


l 

t 



* 

nsave =, n , 

icounter = 0 

, 1 icounter = icounter + 1 

if ( nsave . ge. 100 ) then 
nsave = nsave/10 
j goto 1 

end if 

icoc = nsave/10 
imod = mod (nsave. 10) 

el_cod igo ( is: i s+1 ) “ char (27) //char (91 ) 
if ( icoc . eq. 0 ) then 

el_codigo(is+2: is+3) “ char (48+nsave > //char (68) 

ie = is + 4 

return 

else 

el_cod igo! is+2: is+2) ■ char (4B+icoc ) 
el_codigo< is+3: is+4) - char (48+imod ) //char (68) 
ie - is + 5 
return 
end if 
end 
c 

subroutine cursor home( el_codigo. is. ie ) 

c <ESC>tH 

character*!*) el_codigo 

el_codigo( is: is+2) « char (27)//char (91 )//char (72) 
ie o is * 3 
return 
end 
c 

' subroutine rnter_alt_char_set< el codigo. Is. ie ) 

c <ESC>( 1 

character*!*) el_codigo 

' el_codigo( is: is+2) « char (27)//char(40)//char(49) 

ie *= is + 3 
return 
end 
c 

subroutine exit_alt char set! el codigo. is. ie ) 

c <ESCX0<E5C>C 1 lm 

character*!*) el_codigo 

el_codigo( is: is+2) « char (27) //char (40) //char ( 48) 
is »■ is +3 

call exit_graph_mod*( el_codigo, is. ie ) 
return 
end 
c 

subroutine enter_alt_char_graph set! el codigo. is. ie ) 

c <ESC>!2 

character*!*) el_codigo 

el_codigo(is: is+2) b char (27 ) //char (40)//char (50) 
ie = is + 3 
return 
end 
c 

subroutine e x i t_al t_char_graph_set ! el_codigo. is. ie ) 

c <ESCX0<ESC>C 11m 

character*!*) el_codigo 

el_codigo( is: is+2) = char (27 ) //char (40) //char (48 ) 
is = is + 3 


I 


t 

t 

( 

t 

t. 

t 


1 

l 

( 

( 

< 

( 



call e* i t_gj-aph_mode ( el_codigoi is. ie ) 
return 
end 
c 

subroutine cursor off! iunit ) 

c <ESC>C>Sh 

character*10 el_codigo 

el_codigo(l: 5 )=c bar <27 ) //char (91 > //char (62) //char ( 53) //char ( 104) 
write(iunit) el_codigo 
return 
end 
c 

subroutine cursor on( iunit ) 

c <ESC>t>51 

charac ter*10 el codigo 

el_codigo(l: 5)«char (27>//char (91 >//char (62) //char ( 53) //char ( 108) 
uritc(iunit) el_codigo 
return 
end 
c 

subroutine save_cursor _position( el codigo. is. ie ) 

c <ESC>t e 

character*!*) el_codigo 

el_codigo( is: is+2) m char (27)//char (91 )//char( 115) 
ie “ is + 3 
return 
end 
c 

subroutine cursor to save_position( el codigo. is. ie ) 

c <ESC>Cu 

character*!*) el_codigo 

■l_cod igo( is: is+2> •« c har (27 > //char (91 ) //char ( 1 17) 

ie *» is + 3 

return 

end 


€ 


V 






Subrouting INITIALIZE 


Last revised bg Bor-Jau Hsiah (Andy) & 

Juan J. Rodr iguez-Moscoso (Jun-22-8S) 

SUBROUTINE INITIALIZE < Nrt, ndim, tO. tf. dal tat. Y ) 


COMMON AREAS DEFINITIONS AND DIMENSIONS ARRAYS 


IMPLICIT DOUBLE PRECISION (A-H.O-Z) 


Common 

COMMON 


Areas Definitions: 
/ FLAG / FLAG (7) 



COMMON /READIN/ 

Bound ( 19) 

f 

Yin(10> 

• 

Kp <3. 3) 

4- 



Kd <3. 3) 

t 

INMAT(3. 3) 

0 

INMATVI3.3) 

4- 



Thcom(3> 

# 

Omcom(3) 




COMMON / STEP 

/ 

PM0(3> 

f 

RTini (3) 

0 

RTend (3) 

4- 



DT<3> 

$ 

ST(3) 

0 

PT<3> 

4- 



RT(3> 

t 

RTimlnO) 

0 

RTemln(3) 

4 * 



DTmln (3) 

i 

Thpeak(3) 




COMMON / FRQ1 

/ 

Amp 

• 

Omega 

0 

Phase 

4 - 



ts 






COMMON / FRQ2 

/ 

THAMP ( 3 ) 

0 

THFEEC3) 

0 

THAOLO) 

+ 



THP0L(3) 

0 

THS(3) 

0 

THC ( 3 ) 

4* 



XU3) 

0 

X2(3) 

0 


4- 



OMAMP ( 3 ) 

0 

0MFEE(3) 

0 

OMAOL ( 3 ) 

4- 



0MP0LC3) 

0 

OMS ( 3 ) 

0 

OMC ( 3 ) 

4- 



X3(3) 

0 

X4(3) 

0 

XN 


COMMON / FRQ3 

/ 

Freg 

i 

Const 

0 

Tau 


COMMON /PI 

/ 

PI 






DIMENSION 


Y( 10) 

0 

Ytemp(lO) 




LOG I CAL* 1 


FLAG 






REALeS 


Kp . Kd 

$ 

INMAT . INMATV 



COMPUTATIONS 


DO 1-1, 10 

Ytemp(I) - Yin(I) 

END DO 

IF < FLAQO) ) 

THEN 

IF ( Nrt . EO 1 ) THEN 
DO i-i. 10 

Y(i) = Ytemp(i) 
END DO 

IF < FLAG < 4 ) ) THEN 
ndim « 7 


! Store initial states. 


! IMPULSE Response. 

! Reset initial states. 

! Check Quaternion. 


CALL JNIT QUATERN < Y ) 

END IF 
END IF 

tO - <Nrt-l>*<tf+deltat> + <2-Nrt)*tO 
tf - (Nrt-1 )*Bound <2) + <2-Nrt ) /Bound (7) 
DO k«*l. 3 

Thcom(k) « <2-Nrt)*Bound(7)*Bound<k+9> 
Omcoml k ) ■ 0. OdO 
END DO 
RETURN 
END IF 

Y< 1) ■ YtempI 1) 

Y( 2) - YtempI 2) 

Y ( 3) «* YtempI 3) 

Y< 4) - Ytemp ( 4) 

Y< 3) - Ytemp I 31 

Y ( 6> » YtempI 6> 

Y< 7) - Ytemp ( 7) 

Y I B) - Ytemp < 0> 

Y( 9) - Ytemp ( 9) 

YUO) - Ytemp (10) 


IF ( FLAQ(4> ) 

THEN 
ndlm 7 

CALL INIT QUATERN < Y > 
END IF 


! Check Quaternion 


IF < FLAOU) ) RETURN 

DO I - 1, 3 

THC II) - 0. dO 
THS(I) - 0. dO 
OMC( I ) - 0. dO 

onscn - o. do 

END DO 

Fren " Freq * Conet 
Period ■ 1. dO/Freq 
deltat - 1. dO/Bound<3> 
netep - INTI Period/deltat ) 

Period - DFLOATI netep ) • de 
Omega “ 2. dO * PI / Period 

te » 6. dO * Tau 

tf ■ 6. dO * Tau ♦ Period 
RETURN 
END 


! STEP Reeponee. 

! FREQUENCY Reeponee. 
! Clear Temporary 
! Storage*. 


! Compute current frequency. 

! Compute Period. 

! deltat ie filed. 

! Compute integer # of 
! deltat'e in 1 period 
tat ! Truncate Period. . 

! Compute Angular Freq. 

! Compute Integrate 
! etart time. 

! Compute Integrate 



c I Copyright (c) NASA Marshall Space Flight Center t 
c I Hunstville. Alabama I 
c I 1985 t 
c 

c »»»***»##••****•**»*»**»•*•»•#*»•»**•**»#**»•«•»«**••»*«»»»»•»••*»****# 
c * * 
c * Subroutine INIT_QUATERN * 
c * * 
c *********»************#*********#*###*«*****»**##**«#**#**»*#********** 


c *** This routine computes the initial quaternion given the initial 
c Roll-Pitch-Yaw angles, or Euler angles, in terms of the V frame. 

c *** Written by Juan J. Rodr iguez-Moscoso (May-02-85) 
c Revised on May-12-85 

c on May —13— 05 : Saving initial values RPY. 

c on May-18-85 : Cleaning up the routine, 

c on Jun-02-85 : Program restructure. 

SUBROUTINE IN1T QUATERN ( Y ) 


c« COMMON AREAS AND DIMENSION ARRAYS 

IMPLICIT DOUBLE PRECISION (A-H. 0-Z) 
c »»# Dimensioning of Arrays 

DIMENSION 00(4). ! Initial Quaternion Storage 

+ Y(10) ! Vector of states 


c* COMPUTATIONS 


c *** The initial RPY angles are stored in Y(7). Y<8). and Y(9) of the 

c Y(i) array. The initial quaternion will be stored in GO(i> where 

c i“l«2. 3. and 4. After quaternion's computations, the initial qua- 

c ternion will be stored in Y(i)» i»4.5,6.7. 

c Computing Scalar Part of the initial Quaternion: Q0(4). 

QO< 4) = DC0S(Y<7>/2. D0)*DC0S(Y(8)/2. D0)*DC0S(Y(9) /2. DO) - 
DSIN(Y(7)/2. D0)*DSIN<Y<8>/2. D0)*DSIN(Y(9) /2. DO) 

c Computing Vector Part of the initial Quaternion: QO(i) Ci-1.2.31, 

QO( 1) = DSIN(Y(7)/2. D0)#DC0S(Y(8)/2. DO)*DCOS< Y(9) /2. DO) + 
DC0S(Y(7)/2. D0)*DSIN(Y(8)/2. D0)*DSIN(Y(9) /2. DO) 

Q0( 2) = DSIN(Y(B)/2. D0)*DC0S(Y(7)/2. D0)*DC0S(Y(9)/2. DO) + 
DC0S(Y(8>/2. D0)*DSIN(Y(7)/2. D0)*DSIN(Y(9) /2. DO) 

QO( 3) - DSIN(Y(9)/2. D0)*DC0S(Y(7)/2. D0)*DC0S<Y(8)/2. DO) - 
DC0S(Y<9)/2. D0)*DSIN(Y<7)/2. DO)*DSIN(Y(B)/2. DO) 

c **» Saving initial values of Theta (RPY) 

Y( 10) = Y ( 9) 

Y( 9) = Y ( B) 

Y ( 8) = Y ( 7) 

c *** Storing the initial Quaternion into Y(i) C i“4. 5i 6. 73 
Y( 7) =■ Q0( 4) 



t 


4 


V 




aN3 

NHfU3d 

<1 )00 = ( tr ) A 
(2 >00 = (S > A 
' CE >t)0 = (9 ) A 



Copyright (c) NASA Marshall Space Flight Center 
Hunstville. Alabama 
19B5 


c •*•»*»«•**•»*****»»»**»»»*»»«»•*»*«»***«*******»••*»»»»•»*««*»*«*•*•*»* 
c * * 
c * SUBROUTINE INTEGRATION * 
c * * 
c #****#*#*#*#***********#****e************#******#*****»*******#e******* 


c *** 

c 

c 

c 

c 

c ••• 

c 

c 

c *** 
c 


Written by Juan J. Rodr iguez-Mos coso & 

Bor-Jau Hsieh (Andy) (May-02-85) 

Revised on May-06-B5 

on May-12-B5 : Adding aui. Return 

on Jun-02-B5 : Program restructure. 

This routine checks out uhat type of integration method has been 
selected by the user. Them it makes the appropriate calls to 
the integration routines. 

The type of integration method is indicated by: 


c 

If 

FLAG (5) - 

. TRUE. 

then 

EULER 

c 

If 

FLAG ( 6 ) » 

. TRUE. 

then 

RUNGE-KUTTA 

c 

If 

FLAG (7) - 

. TRUE. 

then 

PRED I CTOR-CORRECTOR 


SUBROUTINE INTEGRATION ( ndim. to. tf, deltat. Y, Ydot. *. * ) 


< 

< 


COMMON AREAS AND DIMENSION OF ARRAYS 


. Kp<3.3> 

. INMATV(3.3) 


DIMENSION Y( 10) , Ydot(lO) 

REAL*B Kp , Kd . INMAT . INMATV 

LOGICAL*! FLAG 


IMPLICIT DOUBLE PRECISION (A-H. O-Z) 

COMMON /READIN/ Bound (IS) . Yin(lO) 

♦ Kd (3. 3) , INMAT (3> 3) 

+ Thcom(3> i Omcom(3) 

COMMON / FLAO / FLAG (7) 


c* INTEGRATION 

c 

c *** Checking out type of integration to be applied. 

c EULER Integration uhen FLAQ(5) - .TRUE. 

IF ( FLAG ( 3 ) ) 

THEN 

CALL EULER ( ndim, to. tf. deltat. Y. Ydot > 

RETURN 
END IF 

c RUNGE-KUTTA FOURTH ORDER Integration when FLAQ(6> - .TRUE. 

IF ( FLAQ(6> ) 

THEN 

EPS = Bound! A) 

CALL RUNGE_KUTTA ( ndim. tO. tf. deltat. EPS. Y. Ydot. MOO ) 
RETURN 



I 


END IF 


c PREDICTOR-CORRECTOR Integration uhen FLAG(7> - .TRUE. 

IF < FLAG(7> > 

THEN 

EPS = Bound ( 4) 

CALL PRED CORREC ( ndim, tO. tf. deltat. EPS. Y. Ydot. S.110 ) 
RETURN 
END IF 

c — - RETURN cadet in cate of Integration problem*. 

100 RETURN 1 

1 10 RETURN 2 

END 



c 
c I 
c I 
c : 
c + 





— — — — — — — — — —— — ______ — — — — ——————— ———+ 
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c •*«»••«»***«»••««»«»»•»*»•»***«««**«*•*«*•»•*«»*•»••»«*****»**#•***•**» 
c * * 
c * GENeric SIMulation program IFQRTRAN-77 portion of NESS) * 
c * * 
c *»•«*»*••«**•»»*«••««•»»**•*•**•«••*****#******««#**«•«**«***«***•#***» 


»## 


This program has been designed for solving in a generic form the 
simulation problem stated by the following set of equations: 
PROTOTYPE 0: 


Torque! i ) = Kpli>j)*t Thcom( i )-Thact ( i ) 3 h 
K d(i,j)*C Omcoml i )-Omact ( i ) 3 

Ydot(i) » INMATV! i. j)*C Torque(i) - 

Omega(i) X INMAT! i. j ) sOmega ( i ) 

with the assumptions: 

Steering Dynamics : Unity 
Actuator & Torque : Unity 
Sensors : Unity 

PROTOTYPE 1: 


iConTRoLLER) 


iBDdy 

DYnamics) 


Gdot(i) “ F! t> Q> Qmegi 
Theta_v!i) - 0! CVR3 > 
DESCRIPTION OF INPUT VARIABLES: 


_v ) (Quaternion 

rate-equations) 
!Rotation Matrix VR to 
determine Thetav) 


SSS3B=a 




c 



Step Response 

Freq. Response 
_ 


Bound! i) 

= 

to 

to 

iinitial time) 


! 2) 

a 

tf 

Tau 

ifinal time & tau) 


! 3 ) 

a 

deltat 

1/deltat 

! increment time) 


! 4) 

a 

Epsilon 

Epsilon 

lerror for Integ) 


( 5) 

a 

0. 0 

0. 0 

IStop Condition) 


! 6 ) 

a 

X Steady S. 

ANLZ axis 

ISteady State E. Sc 






axis for analyze) 


! 7) 

m 

blank 

blank 



! 8 ) 

a 

blank 

Amplitude 

! Inp. Sinu. Amp. ) 


! 9 ) 

a 

balnk 

Omeqa..lo 

! lowest Freq. ) 


! 10) 

=* 

□mega x !Inp) 

Phase 

!Omega_x Input & 






Phase for FRO) 

c 

! 11 > 

a 

Omega_u " 

Ndec 

!Omega_y Input & 






< # of decades) 


! 12) 

3 

Omega_i " 

Nsd 

!Omega_z Input Si 






(# samp, f/dec. ) 


< 13 ) 

= 

Theta_x 

Theta x 

( Inp. axis “ 0, 1 


( t X ! 

= 

Theta_y " 

Theta_y 

for FRQ» any value 


! 1 1 . > 


Theta_i M 

Theta_x 

for STP. ) 




PROTOTYPE 0 

PROTOTYPE 1 



Y! 1) 

a 

Omega_x <0) 

Omega_x !0) 

Iinitial Omega_x) 


c , < 2) .= 0mega_g(0) . 0mega_g(0) 

c < 3) = Omega_z (0) . 0mega_z(0) 

c < 4) = Theta_x(0) blank 

c ( 5) = Theta_g<0) blank 

c ( b) = Theta_z(0) blank 

c < 7) = blank . ROLL(O) 

c ( 0) = blank . YAW(O) 

c ( 9) = blank . P1TCH<0> 

c (10) = blank . blank 

c *** Written bg Bor-Jau Hsieh (Andg) & 
c Juan J. Rodriguez-Moscoso (Ol-Mag-85) 

c Last revised on 31-Mag-B5 


PROGRAM GENSIM 


COMMON AREAS DEFINITIONS AND ARRAYS DIMENSIONING « 

IMPLICIT DOUBLE PRECISION (A-H. 0-Z) 

COMMON /READIN/ Bound (IS) . Yin(lO) , Kp(3. 3) 

+ Kd (3. 3) , INMAT (3i 3) , INMATVI3.3) 

+ Thcom(3) > Omcom(3> 

COMMON / FLAG / FLAG (7) 

COMMON /PI /PI 

REAL#8 Kp . Kd . INMAT . INMATV 

LOGICAL*! FLAG 


COMPUTATIONS 


c *** Computing PI: 

PI «* DATAN2( 0. ODO . -1. ODO ) 

c **# Reading in the input file: "SIMULA. INP" 

OPEN (unit ** 1. status = 'old'. file ■ 'SIMULA. INP ' > 


READ( It * ) 

FLAG 

! Tgpe of response b integration 

+ 

Bound • 

! Boundaries of conditions. 

+ 

Yin i 

! Initial States. 

+ 

Kp < 

! Kp Matrix. 

+ 

Kd . 

! Kd Matrix. 

+ 

INMAT 

! Inertia Matrix. 


c »»* Closing Input Unit 
CLOS£(unit*=l ) 

c *** Open output files according to specified response tgpe. 

CALL OUTPUT_FILES 

c #** initiate integration routine ! Mag-31-B5 

CALL SIMULATION ( ICODE ) 

c *** Closing files and deletion of F0R017.DAT & F0R018.DAT. 

CLOSE ( unit=>17. DISP= 'DELETE ' ) ! Mag-29-B5 

CALL EXIT 


(Initial Omega_g) 
(Initial Omega_z) 
(Initial Theta_x) 
(Initial Theta_g) 
(Initial Theta_z) 
(Init. Roll angle 
(Init. Yaw angle) 
(Init. Pitch ang. 


4 

4 

4 

4 

< 

< 

« 

4 

(i 

4 

(. 

«. 

k 

x 

< 

( 

4 

4 

( 


< 



I 


c *** Check return codes 

IF < ICODE ) 10,30.20 

c *** If ICODE < 0 then. No Response Analysis was chosen. 

10 CONTINUE 

WRITE<6, 15) 

15 FORMAT ( 1H0, '************** ERROR DETECTED *************** '/1H0. 

+ . NO RESPONSE ANALYSIS WAS CHOSEN '/1H , 

+ ' FORTRAN STOP IS FOUND FROM THIS EXECUTION ') 

STOP 

c •»• If ICODE > 0 then. Error detected during the Integration process 
c or Inversion routine 

20 CONTINUE 

GO TO ( 21, 23, 25 >, ICODE 

21 WRITE<6. 22) 

22 FORMAT ( 1H0, '************** ERROR DETECTED *************** '/1H0, 

' THE INERTIA MATRIX IS NOT INVER5IBLE V1H » 

' FORTRAN STOP IS FOUND FROM THIS EXECUTION. ') 

STOP 

23 WRITE(6, 24) 

24 FORMAT ( 1H0, '************** ERROR DETECTED ***************'/ 1H0. 

' NO CONVERGENCE DURING THE INTEGRATION '/1H . 

' PROCESS FOR RUNGE-KUTTA 4TH ORDER SELECTED '/1H , 

' FORTRAN STOP IS FOUND FROM THIS EXECUTION. ') 

STOP 

25 WRITE (6, 26) 

26 FORMAT (1H0. '************** ERROR DETECTED *************** '/1H0, 

' NO CONVERGENCE DURING THE INTEGRATION V1H , 

PROCESS FOR PREDICTOR-CORRECTOR SELECTED '/1H . 

' FORTRAN STOP IS FOUND FROM THIS EXECUTION. ') 

STOP 

c *** If ICODE * 0 then. Execution suceeded. 

30 CONTINUE 

c ••* End of Simulation. 

END 





•; I 

c I Copyright (c) NASA Marshall Space Flight Center I 

c I Hunstville> Alabama I 

c 1 19B5 I i 
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c *****•«««****««•*«*«»»«««*•****««*««*********•*«*«•**»•»•«*«»«»****««*« 
c * * 
c » SUBROUTINE MATINV * 
c * * 
c ***#■»*****«*#**.*****##»***#.***#**■*##■»■»*■»*#*#*#*****#*#»*#*.»♦»**#***#»** 


< 




c *** Subroutine written by Juan J. Rodriguez-Moscoso (18-Apr-BS) 

SUBROUTINE MATINV C A. N. ICODE ) 

IMPLICIT DOUBLE PRECISION (A-H.O-Z) 

DIMENSION A(N.N). MU<25> 

ICODE = 0 
DO 5 1=1. N 
MU( I ) = I 
5 CONTINUE 

DO 100 1=1. N 
IP1 = 1+1 

IF ( IP1 .LE. N > THEN 
L = I 

AMAX = DABS( A(I. I) ) 

DO 30 K-IP1.N 

IF ( AMAX .LT. DABS( A(K< I ) ) ) THEN 
L = K 

AMAX = DABS( A(K. I) ) 

END IF 

30 CONTINUE 

IF ( L .EQ. I ) GOTO 50 
K = MU( I ) 

MU(I) = MU(L) 

MU(L) = K 
DO 40 J=l» N 
P = A(I.J) 

A(I.J) = A (L. J) 

A(L. J) = P 


40 

END 

CONTINUE 

IF 

50 

P = 

A<I. I) 

C ««• 

Testing 

Singularity 


IF< DABS<P ) -LE. 0. 20D-16 > THEN 
ICODE = 1 
RETURN 
END IF 

C •*** Computing the inverse of the A matrix 

DO 60 J=1 « N 

A(I. J) = A(I, J)/P 
60 CONTINUE 


t 



70 

80 

90 

100 

no 

120 

130 

140 


DO 80 K=1«N 

IF< K .EQ. I ) GOTO 80 
DO 70 J=1.N 

IF ( d . EQ. I ) GOTO 70 
A(K< J) = A(K. J)-A(K) I )*A( I< J) 
CONTINUE 
CONTINUE 

DO 90 K=t.N 

A(K, I) - -A(K. I)/P 
CONTINUE 

A(I. 1) » -A(I. I > 

CONTINUE 

DO 140 J=l» N 

DO HO K-J, N 

IF < MU<K) .EQ. J ) GOTO 120 
CONTINUE 

IF< K .NE. J ) THEN 
MU(K) = MU( J> 

DO 130 1=1. N 
P = A( I. K) 

A(I.K) = A(I.J) 

A(I.J) ■ P 
CONTINUE 
END IF 
CONTINUE 


RETURN 

END 




c •*••*««*****««««*#•*•**«»**»*«•«**#*«**•*«*«**«**«*#««*•«*«*«#****»**** 
c * # 
c * Subroutine NEW_VALUES » 
c * * 
c *********************************************************************** 


c *** 


This subroutine performs the follouing operations: 

1. - Normalization of the Quaternion bg applging the 

follouing relations: 

N - DSQRT*( Ql**2+Q2**2+Q3**2+Q4**2 > 

and. 

Q - Qi/N 

2. - Computation of the CVR3 Matrix from the updated 

quaternion. 

3. - Computation of the neu set of angular positions from 

the CVR3 matrix bg applging eq. 2.9 (Attitude Module 
representation paper sent bg K. Fernandez). 


c *** Written bg Juan J. Rodriguez-Moscoso ( 04-Mag-85 ) 
c Last revised on 02-Jun-B5 


SUBROUTINE NEW_VALUES ( ndim. Y ) 


C 


c* COMMON AREAS AND DIMENSION ARRAYS 

IMPLICIT DOUBLE PRECISION (A-H. O-Z) 

COMMON / FLAG / FLAG (7) 

DIMENSION Y(10). ! Vector of states. 

♦ VR <3> 3) ! The IVR3 matrix 

LOGICAL*! FLAG 


c* COMPUTATIONS 



c *** Checking out for Quaternion flag 
IF ( .NOT. FLAG (4) > RETURN 

c •** Computing the quaternion norm and update of Quaternion. 

the_norm - Y< 4>*Y< 4> + Y( 5>*Y< 5> Y( 6)*Y( b > + Y< 7>*Y( 7) 
the_norm ■ DSQRT( the_norm ) *.■ 

Y( 4) ■ Y( 4)/the_norm 
Y< 5) “ Y< 5)/the_norm 

Y< 6) ■ Y( 6)/the_norm v 

Y ( 7) = Y< 7)/the_norm 


c «** Computing the CVR3 matrix 
DO i«1.3 
DO j = li 3 

VR(i. j) >= Y( i+3)*Y( j+3) 
END DO 
END DO 


K, 

c 

v 


(Mag-10) 

(Mag-10) 

(Mag-10) 


Ic 


DO i=1.3 
DO j = l, 


kk 
ksign 
VR < i. j) 
VR ( i. j) 
END DO 
END DO 


! (Mag-10) 

! (Mag-10) 

j*( -&6+i»(98-27*i)+j*(13+i*(-21+fc#i)) ) + 
i*( -1 02+27# i ) + lb 
JIABS( k ) 

JISICNd. k > 

VR(i.j) + DFLOTJ( ksign )*Y(7)»Y ( k k+3> 

2. dO#VR{ i. j ) 


DO 1=1,3 ! (Mag-10) 

VR ( i i i) = VR(i. i) - 1. OdO 
END DO 


c #** Tha sat of values for Theta are extracted from the EVR3 matrix, 

c These values will be stored at Y(l> for 1=8. 9. lO. 

temp = DSORT ( VR(2. 2)*VR(2. 2)+VR(3. 2)*VR(3, 2) ) 

Y ( 8) = DATAN2( - VR(3. 2). VR(2, 2) > 

Y( 9) = DATAN2( -VRd.3), VR(l.l) ) 

Y( 10) = DATAN2( VR(1.2>. temp > 

RETURN 

END 



w V ' 

c I Copyright (c) NASA Marshall Space Flight Center 

c t Hunstville, Alabama 

c I 1985 


c *********************************************************************** 
c * # 
c * SUBROUTINE 0UTPUT_F1LES * 
c * * 
c *********************************************************************** 


c 

c *** Written by Juan J. Rodr iguez-Moscoso ( 02-May-B5 ) 
c Last revised by J. Rodr iguez-Moscoso fc 

c Bor-Jau Hsieh (Andy) on 02-Jun-85 


SUBROUTINE OUTPUT_FILES 


COMMON AREAD DEFINITIONS AND ARRAYS DIMENSIONING 

IMPLICIT DOUBLE PRECISION (A-H. 0-Z) 

c *** Common Areas Definitions: 

COMMON / FLAG / FLAG (7) 

c *** Variables Definitions 

CHARACTER *13 FILEOUT 

CHARACTER* 10 EXTO 

CHARACTER*12 EXT1 

CHARACTER*9 EXT2 

CHARACTER*5 THETA, OMEGA, NUMER 

CHARACTER*3 CHA, PLT. AMPLIT, PHAS. FRQ, IMP. STP. OUT 

CHARACTER*1 ZERO. DOT 

LOGICAL*! FLAG 


c **» Initializing a few variables 

DATA THETA/ 'THETA'/. OMEGA/ 'OMEOA '/. NUMER/ 'NUMER '/. 

+ PLT/'PLT'/. AMPLIT/ 'AMP'/. PHAS/'PHA'/, 

+ FRG/'FRQ'/. IMP/ ' IMP '/. STP/'STP'/. OUT/'OUT'/, 

+ CHA/'CHA'/, ZERO/'O'/, DOT/'. '/ 


COMPUTATIONS 

EXTO « THETA/ /OMEGA 
EXT1 - CHA//PLT//AMPLIT//PHAS 
EXT2 «* STP //FRO/ /IMP 

*** Open Output Files of Simulation. 

The output files are given by the following relationships: 
NUMERICAL OUTPUT: 

-> UNIT » 1 j NUMEROOUT. STP (Step Response) 

-> =2 j NUMEROOUT. FRQ. (Freq Response) 

-> =3 i NUMEROOUT. IMP. (Impulse Response) 

STEP RESPONSE: 

-> UNIT o 7 j THETAOCHA. STP (Characteristics) 
-> = B i THETAOPLT. STP (Plot of Theta) 

-> =9 i OMEGAOPLT. STP (Plot of Omega) 


« 


« 

I 

I 




FREQ. RESPONSE: (RODE 
UNIT = 10 
-> =11 
-> = 12 
-> = 13 

IMPULSE RESPONSE: 

-> UNIT = 14 
-> = 15 

-> = 16 
TEMPORARY STORAGE: 

-> = 17 

PRINTER OUTPUT: 

-> = 18 


plotting > 

i THETAOAMP. FRQ 
i THETAOPHA. FRQ 
l OMEGAOAMP. FRQ 
l OMEGAOPHA. FRQ 

l THETAOCHA. IMP 
( THETAOPLT. IMP 
i OMEGAOPLT. IMP 

i F0R017. DAT 

» PRINTER. DAT 


(Amplitude of Theta) 
(Phase of Theta) 
(Amplitude of Omega) 
(Phase of Theta) 

(Characteristics) 
(Plot of Theta) 

(Plot of Omega) 

(Time series data) 

(Printer Output) 


DO i = 1. 3 

FILEOUTd: 10) = NUMER// ZERO/ /OUT/ /DOT 
IF ( FLAC(i) ) THEN 

11 = 3*i - 2 

12 « il + 2 

iunit = i*(3+i)/2 + 4 ! (May-04-B5) 

FILEOUT (11: 13) = EXT2(il:i2) 

OPEN( unit=i< status 1 * 'new'. name*FlLEOUT ) 


k = i*( 4-i ) ! (May-04-B5) 

DO j-1. k 

jO = j#( -155+j*(75-10*j> )/6 +16 ! (May-04-B5) 

jOO = jO + 4 

jl - (i-2)*(i-2)*( j*(15-3*j)/2-5 ) + ! (May-04-B5) 

( i-1 )*(3-i )*( j*( 34+ j*(-13+ j*2) )-14 ) 
jll - jl + 2 

FILEOUTd: 5) - EXTO( jO: jOO) 

FILE0UT(7: 9) = EXTKJl: Jll) 
iunit = iunit + 1 

OPEN( unit-iunit, status- 'neui name-FILEOUT ) 

END DO 
END IF 
END DO 


OPEN( unit-17, status- 'new ' ) 

OPEN( unit-18, status-'neu'. name- 'PRINTER. DAT' ) 


RETURN 

END 



I * 

c I Copyright (c) NASA Marshall Space Flight Center 

c 1 Hunstville. Alabama 

c I 1985 


c #*****##*#*#»****#**#*####*##**#*#*#*###*#**#**#*###*##*#***»#**#**###* 
c * * 

c * Subroutine 0UTPUT_17 # 

c * * 

c #*##*####«####################*#####*####*##########*####*#*########### 

c *## This subroutine handles all related to output of Simulation. It 
c is recording partial results, 

c 

c Written by Juan J. Rodr iguez-Moscoso ( 22-Jan-B5 ) 

c Last revised on 13-May-B5 

SUBROUTINE 0UTPUT_17 ( ndim. t. deltat. Y. Ydot 1 

c# COMMON AREAS AND DIMENSIONS ARRAYS * 

IMPLICIT DOUBLE PRECISION (A-H.O-Z) 

COMMON / FLAG / FLAG 

DIMENSION Y( 10) . Ydot(lO) 

LOG I CAL# 1 FLAG (7) 

c* COMPUTATIONS # 

c No temporary data generated for FREQUENCY response. 

IF < FLAG (2) ) RETURN 

c ### Checking out if "t" is ■ to "t + m*deltat" 
c RES « DMODi t. deltat ) 

c IF ( RES . 6T. 0. ID-12 ) 

c THEN 

c IF ( DABS( RES - deltat ) . OT. 0. ID-12 ) RETURN 

c END IF 

c ### Checking FLAG(4) for Quaternion 
IF < FLAG ( 4 ) > 

THEN 

WRITE(17i#) t , • current timei 

+ ( Y(j>, jol> 10 ) ! Omega and Theta 

RETURN 
ELSE 

WRITEU7.#) t. ( Y( j )i joi, ndim ) 

RETURN 
END IF 




END 


c + 
c I 
c I 
c i 
c + 


Copyright <c) NASA Marshall Space Flight Center 
Hunstville, Alabama 
1985 


c «**«***•#***«***»*««**»«*«*«**«*«*****»*«#*«***««***«*««*«****•«*«»***• 
c * * 
c * SUBROUTINE PREDjCORREC * 
c * * 
c •#**•»****#****»•««««*»»«*#***•*#«#*#*•«*»**«*#**##«**#•****#*»*****«** 


c »** Subroutine to solve a system of first order ordinary general dif- 
c ferential equations with given initial values. 

c **e This method is based on Hammings modified Predictor-Corrector me- 

c thod. This is a fourth order method using 4 proceeding points for 

c computation of a new vector Y of the dependent variables, 

c The adjustment of the initial increment and computation of the s- 

c tarting values is done by a Fourth-order Runge-Kutta method sug- 

c gested by Ralston. 

c *** Written by Juan J. Rodriguei-Moscoso on 21-Apr-B5. 
c Last revised on Ol-Jun-85 


SUBROUTINE PRED_CORREC ( ndim, tO. tf. tincr. EPS, Y, Ydot. * ) 


c« COMMON AREAS AND DIMENSION OF ARRAYS • 


IMPLICIT DOUBLE PRECISION <A-H.O-Z) 

DIMENSION Y( 10) , Ydot(lO) . aux (16, 10) 


c» COMPUTATIONS 

c 

n - 1 
ihlf - O 

t = to 

tend = tf 
deltat = tincr 
I CODE * 0 



DO 1=1. ndim 

aux ( 16, i ) = O. ODO 
aux (15, i) = Ydot(i) 
aux ( 1. i ) - Y( i > 
END DO 


IF( deltat*( tend - t ) ) 30, 20. 40 
20 ihlf = 12 

GO TO 40 

30 ihlf = 13 

40 CONTINUE 

CALL SYS_DYNAM f dim, t. Y, Ydot ) 

CALL OUTPUT 17 ( <i m , t. tincr. Y. Ydot ) 

CALL CHECK ERROR ' ihlf, ICODE ) 

IF < ICODE . NE. C RETURN 1 
IF ( ihlf .GT. 0 ) RETURN 


c 

l 





DO i*>l,nd4fli , 
aux (8. i ) = Ydot( i ) 

END DO 

isw = 1 
GOTO 1000 

90 t ■ t + deltat 

DO isi.ndiin 

aux (2, i ) - Y< i ) 

END DO 

110 ihlf - ihlf + 1 

t * t - daltat 
DO i»i in dim 

aux (4, i) « aux (2> i) 

END DO 

daltat » 0. 5d0*deltat 
n - 1 

isw = 2 
GOTO 1000 

130 t » t + deltat 

CALL SYS_DYNAM ( ndim, t. Y. Ydot ) 
n ** 2 

DO i**l,ndim 

aux(2. i) » Yti ) 
aux (9i i) >* Ydot(i) 

END DO 
isw » 3 
GOTO 1000 

150 DELT - 0. ODO 

DO i*>l>ndiin 

DELT - DELT + aux (15. i )»DABS( Y< i >-aux <4, 1 ) ) 

END DO 

DELT » 0. 066666666666666667*DELT 
IF ( DELT - EPS ) 190, 190. 170 
170 IF ( ihlf - 10 ) 110, 180, 180 
180 ihlf - 11 

t ■ t + deltat 
GO TO 40 

190 t *» t + deltat 

* CALL SYS_DYNAM ( ndim, t. Y, Ydot ) 

DO i=l»ndim 

aux (3. i) - Y(i) 
aux ( 10. i) « Ydot ( i ) 

END DO 
n “ 3 
isw a 4 
GOTO 1000 

210 n ° 1 

t » t + deltat 

CALL 5YS_DYNAM ( ndim, t. Y. Ydot ) 
t = to 
DO i»l,ndim 

auxdl, i) - Ydot(i) 

Yti) » aux(l.i) + deltat»< O. 375d0*aux (B, i ) + 
+ 0. 79166666666666667*aux<9. i) - 


/ 


< 




+ 

+ 

230 


270 


290 


c **# 

c 

c 

1000 


+ 


+ 

+ 


* 0.‘ 20B33333333333333*aux ilO, i) + 

0. 04 166666666666667* Ydoti i ) ) 

END DO 

t = t + deltat 
n = n + 1 

CALL SYS_DYNAM < ndim. t. Y. Ydot ) 

CALL 0UTPUT_17 ( ndim. t. tincr. Y. Ydot ) 

CALL CHECK_ERROR ( ihlf. I CODE ) 

IF ( I CODE .NE. 0 ) RETURN 1 
IF ( n . CE. 4 ) OOTO 2000 
DO i«l, ndim 

aui (n. i ) ■ Y< i ) 
auxtn+7. i> - Ydot(i) 

END DO 

IF { n - 3 ) 270. 290. 2000 
DO i**l,ndim 

DELT - 4. OdO*au< (9. i ) 

Yti) - auxil.i) + dtl tat*( aux(B. i )+DELT+aux 110. i) >/3. dO 
END DO 
GO TO 230 

DO i»l,ndim 

DELT - aux (9. i > + lux(lO.l) 

DELT - DELT + DELT + DELT 

YU) - auxU. i)+0. 373d0*deltat*l aux (B, i )+DELT+aux (11, i ) > 

END DO 
GO TO 230 

The following part of the routine computet by meant of Runge- 
Kutta method the ttarting valuet for the not telf-ttarting 
Predictor-corrector method. 

CONTINUE 
DO i=l.ndim 

i *» deltat»auxin+7. i) 
aux (5, i ) * z 

YU) “ auxin, i) + 0. 4d0*i 

END DO 

z = t + 0. 4d0*deltat 

CALL BYS_DYNAH ( ndim, z. Y. Ydot > 

DO i*>l.ndim 

z «= deltateYdot(i) 
aux (6, i ) ■ z 

YU) ■ auxin. i)+0. 29697760924775360d0*aux < 5, i) + 

0. 1 5B75964497 1 03583d 0*z 

END DO 

z t + O. 45373725421B7B943d0»deltat 
CALL SYSJDYNAM i ndim. z. Y. Ydot ) 

DO i**l.ndim 

z *» deltat*Ydot(i) 
aux i7. i ) » z 

YU) » auxin. i)+0. 21810038822592047d0*auxi5, i)- 
3. 05096514B692930B0d0*aux 16, i > + 

3. 8328647604670 1030d0*z 

END DO 

z «= z + deltat 



CALL SYS_DyNAM ( n,dim. z. Y. Ydot ) 

DO i=l,ndim 

Y ( i ) = aox(n, i)+0. 1747602B226269037d0*aux (5, i >- 
+ 0. 55148066287873294d0*aux (6. i > + 

+ 1. 20553559939652350d0*aux (7, i ) + 

+ 0. 171 lB47B121951903d0*deltat*Ydot ( i ) 

END DO 

GO TO < 90, 130. 150. 210 ). iiui 
2000 istep * 3 

2010 IF ( n - 8 ) 2040, 2020, 2040 
2020 DO n*>2, 7 

DO i = l, ndim 

aux(n-l,i) = auxin, i) 
au«(n+6, i) «• aux(n+7, i) 

END DO 
END DO 
n “ 7 

2040 n m n + 1 

DO i»l,ndim 

auxln-1, i ) *» Y( i ) 
aux(n+6. i) = Ydot(i) 

END DO 

t = t +• deltat 

2060 istep *= istep + 1 

DO i^l.ndim 

DELT = aux (n-4, i ) + 1. 3333333333333333*deltat*( aux(n+6. i) + 
+ aux (n+6, i ) - aux(n+5, i) ♦ aux(n+4, 1) + aux(n+4, i) ) 

Y(i> «* DELT - 0. 9256198347107438d0*aux ( 16, i ) 
aux (16. i) «» DELT 
END DO 

CALL SYS_DYNAM ( ndim. t. Y. Ydot ) 

c »** Derivative of Modified Predictor i» generated in Ydot. 

DO ie>i,ndim 

DELT » 0. 125d0*< 9. d0«aux (n-1. i )-aux (n-3. i ) + 

+ 3. dO*deltat*( Ydot ( i >+2. dO«aux (n+6. i >-aux (n+5, i ) ) ) 

aux (16, i) = aux (16, i> - DELT 
Y(i) » DELT + 0. 0743B01652B925620d0*aux(16, i) 

END DO 

c **# Testing whether “deltat" must be halved or doubled. 

DELT - O. ODO 
DO i=>l,ndim 

DELT *■ DELT + aux ( 15. 1 )*DABS( aux(16.i) ) 

END DO 

IF ( DELT - EPS ) 2100. 2220, 2220 

c *#* “deltat" cannot be halved, which means that Y(i) are good. 

2100 CONTINUE 

CALL SYS_DYNAM ( ndim. t. Y. Ydot > 

CALL 0UTPUT.17 ( ndim, t. tincr. Y. Ydot ) 

CALL CHECK ERROR ( ihlf, ICODE ) 

IF ( ICODE . NE. 0 ) RETURN 1 
IF ( ihlf . GE. 11 > RETURN 

IF ( deltat*( t - tend ) . GE. 0. OdO ) RETURN 

IF ( DABS( t - tend ) . LT. 0. ldO*DABS( t ) ) RETURN 


c 
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IF ( DELT . GT. 0. 02d0*EPS ) GOTO 2010 
> > 

c *»» "deltat" might be doubled if all necessary values are available. 

IF < ihlf . LE. 0 > GOTO 2010 

IF < n . LT. 7 ) GOTO 2010 

IF ( istep .LT. 4 ) GOTO 2010 

imod = istep/2 

IF ( istep . NE. imod + imod ) GOTO 2010 
deltat » deltat + deltat 
ihlf = ihlf - 1 
istep = 0 
DO i*=l. ndim 

aux(n-l.i) » aux(n-2. i) 
aux (n-2, i) “ aux(n-4. i> 
aux(n-3. i) * aut(n-6, i) 
aux(n+6. i) - aux(n+5ii) 

•ui(n+5, i) - aux(n+3, i) 
aux(n+4, i> ■ aux(n+l»i) 

DELT ** aux(n+6. i) + aux (n+5. i) 

DELT - DELT + DELT + DELT 

aux ( 16. i > « B. 962962962962963d 0*( Y< i )-aux (n-3. i ) > - 
+ 3. 361111111111111d0*( Ydot ( i )+DELT+aux (n+4. i) )• 

+ deltat 

END DO 
GOTO 2010 

c •** "deltat" has to be halved. 

2220 ihlf « ihlf + 1 

IF ( ihlf .OT. 10 ) GOTO 2100 
deltat «> 0. SdOedeltat 
istep •> 0 
DO i*=l, n dim 

YCi) - 0. 390625d-2#( 8. dl*aux (n-l< i )+135. dOeaux (n-2> i )+4. dl* 

+ auxtn-3, i)+aux<n-4, i) ) - 0. 1171B75dO*( aux<n+f>. i)- 

+ b. dO*aux <n+5» i) -aux (n+4» i) )*deltat 

aux (n-4. i ) = 0. 390625d-2*< 12. d0*aux (n-1. i )+135. dOeaux (n-2. i ) 
♦ +108. d0»aux(n-3, i)+aux<n-4. i) ) - 0. 0234375d0*< 

+ aux (n+6i i ) + lB. dO«aux (n+5. i )-9. dO*aux (n+4. i ) )» 

+ deltat 

aux(n-3<i) » aux (n-2. i) 
aux(n+4. i) = aux(n+5.i) 

END DO 

t = t - deltat 

DELT ■ t - < deltat + deltat ) 

CALL SYS_DYNAM ( ndim. DELT. Y. Ydot ) 

DO i»l.ndim 

aux (n-2. i ) ■ Y( i ) 
aux(n+S. i) = Ydot(i) 

Y( i ) = aux < n-4. i ) 

END DO 

DELT = DELT - 2. OdO*deltat 

CALL SYS_DYNAM ( ndim. DELT. Y. Ydot ) 

DO i=l.ndim 

DELT = 3. D0*< aux (n+5. i )+aux (n+4. i ) ) 

aux ( 16. i ) » 8. 962962962962963d0*( aux (n-1 . i >— Y< i ) ) - 

3. 36111111111UlldO*( aux(n+6. i )+DELT+Ydot( 1 ) >* 
deltat 


+ 

+ 


9 


O 


c 
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C + 

c I Copyright (c) NASA Marshall Space Flight Center I 
c I Hunstville. Alabama I 
c ! 19B5 I 
c + 

c *********************************************************************** 
c * * 
c * SUBROUTINE POLFRCAL * 
c * ( Pseudo Dpen Loop Frequency Response CALculations ) * 
c * * 


c a********************************************************************** 
c Last revised by Bor-Jau Htieh (Andy) & 

c Juan J. Rodr iguez-Moscoso on 02-Jun-85 

SUBROUTINE POLFRCAL ( deltat ) 


c* COMMON AREAS AND DIMENSION ARRAYS * 

IMPLICIT DOUBLE PRECISION (A-H.O-Z) 

COMMON / FLAG / FLAG (7) 

COMMON / USER / user_choice 

COMMON / FRGl / Amp 

+ ts 

COMMON / FR02 / THAMP ( 3 ) 

+ THPOLO) 

+ XI (3) 

«• OMAMP ( 3 ) 

+ 0MP0L13) 

+ X3(3) 

COMMON /PI /PI 

INTEGER user_choice 

LOGICAL*! FLAG 


c 

c* COMPUTATIONS * 

IF < .NOT. FLAG ( 2 ) ) RETURN 

XN = 2. dO»PI/( Omega*deltat ) 

c Calculate closed-loop amplitude & phase 

DO 10 1 = 1.3 

THS(I) = THS<I>/XN 
THC(I) = THC( I )/XN 
THAMP ( 1 ) =0. ODO 
THFEE(I) = 0. ODO 

IF ( THS(I) .EG. 0. ODO AND. THC(I) . EQ. 0. ODO ) GOTO 5 
THAMP ( I ) = 2. *DSQRT(THS( I )*THS( I J+THC ( I )*THC( I ) ) 

THFEE(I) = DATAN2( THC(I). THS(I) ) 

Pseudo Open-Loop Computations 

THAOL(I) = THAMP < I ) /DSQRT ( THAMP < I ) *THAMP ( I ) 

+ 2. ODO*THAMP ( I ) *DCOS < THFEE ( I ) ) + 1. ODO > 

THAOL(I) = 20. 0D0*L0G10( THAOL(I) ) 

Xl(I) = THAMP ( I )*DSIN( THFEE < I ) ) 


c **• 

+ 


. Omega 

, THFEE(3) 
, THS ( 3 ) 

. X2(3> 

. OMFEE < 3 > 
. 0MS(3) 

. X4(3) 


« Phase 

. THAOL ( 3 ) 
, THC(3) 

OMAOL ( 3 ) 
. OMC (3) 

. XN 
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X2( I ) = THAMP ( I ) »DCOS ( THFEE(I) ) + 1. ODO 
THPOLUi = THFEE(I) - DATAN2( X1(I).X2(I) > 
THPOL(I) «= THPOL< I )*180. DO/PI - 1BO. OdO 


CONTINUE 

OMS(I) = OMS(I)/XN 
OMC(I) ■ OMC ( I ) /XN 
OMAMP ( I ) =0. ODO 
OMFEE(I) = 0. ODO 

IF ( OMS(I) .EQ. 0. ODO .AND. OMC(I) . EQ. 0. ODO ) GOTO 10 
OMAMP ( I ) = 2. *DSGRT (0M5( I ) *DMS( I )+OMC ( I )*OMC ( I ) ) 

OMFEE(I) = DAT AN2 ( OMC ( I ) • OMS ( I ) ) 


Pseudo Open-Loop Computations 

OMAOL(I) «■ OMAMP ( I ) /DSGRT ( OMAMP ( I ) eOMAMP ( I ) 


OMAOL(I) 
X3( I > 
X4( I ) 
OMPOL(I) 
OMPOL(I) 

CONTINUE 


+ 2. eOMAMP ( I )»DCOS(OMFEE( I ) ) ♦ I. 
20. D0*L0C10( OMAOL(I) ) 

OMAMP ( I ) <DSIN<OMFEE< I ) ) 

OMAMP ( I ) *DCOS ( OMFEE ( I ) ) + 1. ODO 
OMFEE(I) - DATAN2( X3(I).X4(I) > 
OMPOL ( I ) * 1 80. ODO/P I - 180. OdO 


ODO ) 


IF ( user_choice . EQ. 1 ) THEN 

MRITE(6. 1000) Omega/ (2. «PI), Omega, deltat. 

THAOL. THPOLi OMAOL. OMPOL 

END IF 

WRITE (17. *) Omega. 

< THAOL(J). THPOL(J). 

OMAOL(J). OMPOL (J). J*1.3 ) 


1000 FORMAT ( 1H0, 4X. O 


5X. '** Current frequency ■> '.012. 5. ' Hz ('.012,5.' r'. 
'ad/sec >i '/1H . 8X. 

' Time increment - ',012.5,' (deltaT)'/lH . 10X. 
'ANGULAR POSITION: '/1H . 10X, 16< '-' )/lH , 18X. 'Amplitude ' 
'(db) '. 19X, 'Phase (degrees ) '/1H . 10X. ' X-a*is '. ' Y-' 

'axis '. ' Z-axis ',2X. ' X-axis ', ' Y-axis '. ' Z' 
'-axis '/1H , 10X,3(G103>.2X,3(G10.3)/1H0. 10X. 

'ANGULAR RATE: '/1H . 10X. 12( '-' )/lH . 1BX, 'Amplitude (db)' 
19X, 'Phase (degrees) '/1H , 10X, ' X-axis '. ' Y-axis ', 
' Z-axis ', 2X> ' X-axis '. ' Y-axis '. ' Z-axis '/ 
1H . 10X. 3(010. 3) . 2X, 3(010. 3 ) > 


t 





c ! Cbpyright’ (c) NASA Marshall Space Flight Center ! 
c I Hunstville. Alabama I 
c I 19S5 I 


c *********************************************************************** 
c * * 
c * SUBROUTINE QUATERNION * 
c * * 
c ************************************************************************* 


c *** This subroutine take* care of QUATERNION'S computation*. The cal- 

c culation* present in this routine involve the computation of the 

c Quaternion-Rate equations in terms of the angular rates in V and 

c R frames. In this 1st phase we consider Yr(i) ** 0.0. 

c *** Subroutine written by Juan J. Rodr iguez-Moscoso on 22-Apr-B5 

c Last revised on 10-May-85 

SUBROUTINE QUATERNION ( ndim. Y. Ydot ) 


c* COMMON AREAS AND DIMENSION ARRAYS 


IMPLICIT DOUBLE PRECISION <A-H, 0-Z) 

COMMON / FLAO / FLAQ(7> 

DIMENSION Y(10). ! Vehicle Theta t< Omega 

+ Ydot(10)» ! Vehicle Theta & Omega Rates 

+ Yr(10) ! Moveable Reference 

LOO I CAL* 1 FLAO 



c* COMPUTATIONS * 


c *** Equating aiplicitly Yr(i) « 0. 0 
Yr < 1> =0. ODO 
Yr ( 2) - 0. ODO 
Yr ( 3) - 0. ODO 
Yr ( 4) = 0. ODO 
Yr ( 5) ■ 0. ODO 
Yr ( 6) « 0. ODO 
Yr ( 7) <* 0. ODO 
Yr < 8) - 0. ODO 
Yr ( 9> «■ 0. ODO 
Yr ( 10) - 0. ODO 

c *** If FLAGI4) *■ .FALSE, then no Quaternion computation is performed. 
IF< .NOT. FLAO (4) ) RETURN 


c *•» Quaternion's computations are done in arrays Yti) and Ydot(i)r 
c previously to Integration, 

c Thus* we have that: 

c Qdot(l) = Ydot (4) - O. 5*C Q<4>*< Yv(l)-Yr(l) ) 

c - G<3>*< Yv(2)+Yr (2) ) 

c + Q<2)*( Yv(3)+Yr <3) ) 3 

c - Qdot (2) = Ydot (S) = 0. 5*t Q<3)*( Yv(l)+Yr<l> ) 

c - + Q(4)*( Yv(2)-Yr (2) ) 


c 

c 

c 

c 

c 

c 

c 

c **# 


, - Q( 1 )*( Yv(3)+Yr (3) ) 3 

Qdot (3) = Ydot (6) = 0. 5*C - Q<2)*< Yv(l)+Yr(l> ) 

+ Q( 1 )*( Yv(2)+Yr (2) > 

+ Q(4)*( Yv(3)-Yr(3) > 1 
Qdot (4) = Ydot (7) = 0. 5*C - Q<1>*< Yv(l)-Yr<l) ) 

- Q(2)*( Yv(2)-Yr (2) ) 

- Q(3)*( Yv<3)-Yr<3) > 3 
Forming the Quaternion Rate equations. 


Ydot (4 > - O. 5d0*< Y<7>*< Y«l>-Yr<l) ) - Y<&>*< Y(2)+Yr«2> ) + 

+ Y<5>*< Y(3)+Yr(3> ) ) 

Ydot (5) - 0. 5d0*< Y(6>*( Y(l)+Yr(l) ) + Y(7)*< Y(2)-Yr(2) ) - 
♦ Y<4>*< Y(3)+Yr (3) ) ) 

Ydot (6) - 0. 5d0*( -Y(5)*< Y(l)+Yr(l) ) + Y(4)*( Y(2)+Yr(2) > + 
+ Y(7)*( Y(3)-Yr (3) ) ) 

Ydot(7> - 0. 5d0*( -Y(4)*( YU)-Yr(l) ) - Y(5)»< Y(2)-Yr<2) ) - 
+ Y<&>*< Y(3>-Yr(3) ) ) 


RETURN 

END 




<L 





.* * 

c I Copyright (c) NASA Marshall Spaca Flight Center 

c I Hunstville. Alabama 

c I 19B5 


C #**##***#***####*#*##***#*#***#*****#*****#**##*##*###***#***#**##**#*1 

c * * 
c * Subroutine RESULTS < 
c * ( 
c a********************************************************************** 


c *** This routine takes the data generated by the OUTPUT routine from 
c the temporary files (unit«17) and builds the output files for 

c each type of response. 

c *** Written by Bor-Jau Hsieh (Andy) and 
c Juan J. Rodr iguex-Moscoso on 12-May-BS 

c Last revised on 02-Jun-85 


SUBROUTINE RESULTS ( Y ) 


c* COMMON AREAS AND DIMENSION OF ARRAYS 

IMPLICIT DOUBLE PRECISION <A-H. 0-Z) 


COMMON / FLAG / FLA0<7> 



COMMON 

/READIN/ 

Bound ( 15) 

# 

Yin(lO) 

« 

Kp<3. 3) 

+ 





Kd(3, 3) 

• 

INMAT <3. 3) 

$ 

INMATV13.3) 

4- 





Thcom<3) 

• 

0mcom(3) 




COMMON 

/ 

STEP 

/ 

PM0(3) 

f 

RTini (3) 

f 

RTend (3) 

+ 





DT(3) 

» 

ST (3) 

» 

PT(3) 

+ 





RT(3) 

$ 

RTimin(3) 

I 

RTemin(3> 

+ 





DTminO) 

I 

Thpeak (3) 




COMMON 

/ 

FRQ2 

/ 

THAMP ( 3 ) 

• 

THFEE(3) 

1 

THAOL ( 3 ) 

+ 





THPOLO) 

» 

THS(3) 

* 

THC(3) 

4- 





XI (3) 

• 

X2(3) 

» 


4- 





OMAMPO) 

» 

0MFEE(3> 

• 

0MA0H3) 

4* 





0MP0L(3) 

$ 

OMS ( 3 ) 

f 

OMC ( 3 ) 

4- 





X3(3) 

$ 

X4(3) 

9 

XN 


COMMON 

/ 

IMPU 

/ 

ST imp (3) 

$ 

PT_imp (3) 

9 

Thpeak_imp (3) 


COMMON 

/ 

PI 

/ 

PI 






COMMON 

/ 

SCRP 

/ 

record_counter 

• 

nduml 

• 

ndum2 


DIMENSION Y(10) 


REAL*8 Kp i Kd , INMAT . INMATV 

INTEGER record_counter < page_counter > pro_type 

LOCI CAL* 1 FLAG 

CHARACTER*5 HOLD 

CHARACTER*? CLS 


c* COMPUTATIONS 


CLS = char (27) //char (91) //char (72) //char (27) //char <91 )//char (50) 
//char (74) 
pro_type = 0 


6 



( 


IF ( FLAGU) > T^EN 
pro_type = 1 
END IF 

IF ( FLAG(l) ) ! STEP Response. 

THEN 

DO J = 1, 3 

IF ( Thcom(J) .NE. 0. ODO > THEN 

PHO(J) = ( DABSCThpeak ( J)-Thcom( J) )/Thcom( J) ) *100. ODO 
RT ( J) - RTend t J> - RTini(J) 

END IF 
END DO 

NUNIT1 *> 1 
NUNIT2 » 7 

ASSIGN 1000 TO NLINE1 
ASSIGN 1010 TO NLINE2A 
ASSIGN 1020 TO NLINE2B 
ASSIGN 1030 TO NLINE2C 
ASSIGN 1040 TO NLINE3 
END IF 

IF ( FLAG<2> ) 

THEN 

nduml •* INT< Bound(ll) 
ndum2 x INT( Bound(12) 

NUN I T 1 - 2 

ASSIGN 2000 TO NLINE1 
ASSIGN 2010 TO NLINE3 
END IF 

IF ( FLAG ( 3 ) ) ! IMPULSE Response. 

THEN 

NUN1T1 - 3 
NUNIT2 - 14 
ASSIGN 3000 TO NLINE1 
ASSIGN 3010 TO NLINE2A 
ASSIGN 3020 TO NLINE3 
END IF 


! FREQUENCY Response. 

) 

) 


c **• 
c 


The next portion of the routine will take care of storing results 
of simulation. 

REMIND 17 

record_counter ** 0 ! Initialising number of records. 

page_counter = 0 ! Initializing number of pages. 


c *** Telling the user to use the NO-SCROLL key to see page by 
c page the output. 

WRITEINUNITI. *) CLS 

CALL ENTER_HOLD SCREEN_MODE ( HOLD, 1. IDUMMY ) 

UR I TE ( NUN I T 1 , * ) HOLD 
WRITEINUNITI, 900) 


c *** Storing values in Numerical Output 
10 CONTINUE 

page_counter “ page_counter + 1 

WRITE<NUNIT1. *) CLS 
URITEINUNITI. NLINE1 ) page_counter 


i => O 



15 


i - i + t l - ! 14 lines/record 

record_counter = record_counter + 1 
IF < FLAG (2) ) 

THEN 

READ(17. *.END=30> FR. TEMPI. TEMP2, 

( Y( j). j-1. 10 ) 

WRITEINUNITl. 940) FR. TEMPI. TEMP2, 

< Y( j). j-1. 10 ) 

i = i + 5 
GO TO 20 
END IF 

IF < FLAG < 4 ) ) 

THEN 

READ! 17. *. END-30) t . ! Reading All 

t Y( j ), j-1. 10 ) ! State. 

WRITE(NUNIT1, 920) t 

< Y(j>. J-1. 3 > . ! Writing All 

( Y(k). k=B. 10 ) ! State. 

ELSE 

READ! 17. *. END-30) t. ( Y(j). j-1.6 ) 

WR I TE ( NUN I T 1 . 920 ) t. ( Y(j). j-1.6 ) 

END IF 

20 IF < i .LT. 14 ) GO TO 15 

WRITECNUNITl. 910) 

WRITEINUNITl. *) HOLD 

GO TO 10 ! Return and write new page 

30 CONTINUE 

c **» Exiting from hold-screen mode. 

CALL EX I T_HOLD_SCREEN_MODE < HOLD. 1. 1 DUMMY > 

WRITEINUNITl. *) HOLD 
IF ( FLAG! 2) > GO TO 40 
IF ( FLAG ( 3 ) ) GO TO 35 

c *** Entering hold-screen mode in unit NUNIT2-7 
WRITE<NUNIT2, #) CLS 

CALL ENTER_HOLD_SCREEN MODE! HOLD. 1. I DUMMY ) 

WRITEINUNIT2. *) HOLD 
WRITE (NUNIT2. 900) 

c #** Storing Characteristic, of Step Respon.e in THETAOCHA. STP 
WR ITEINUNIT2, *) CLS 
WRITEINUNIT2, *) HOLD 

WRITE1NUNIT2. NLINE2A) ( PMO(i). i=1.3 ). 

♦ < PT(i). i-1.3 ) 

WRITE1NUNIT2. *) CLS 
WRITE1NUNIT2. *> HOLD 

WRITE1NUNIT2. NLINE2B) ( RT( i ) . i-1.3 ). 

+ ( DT( i ) . i-1.3 ) 

WRITE(NUNIT2. *) CLS 
WRITEINUNIT2. *) HOLD 

WRITE1NUNIT2, NLINE2C) ( ST ( i ) . i-1.3 ) 

c *** Exiting the hold-screen mode 

CALL EXIT_HOLD_SCREEN MODE< HOLD. 1. IDUMMY ) 

WRITEINUNIT2, *) HOLD 
GO TO 40 



0 
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% 


% 


* 



c **# Storing Characteristics of Impulse Response in THETAOCHA. IMP 
35 CONTINUE 

WRITE (NUNIT2, *) CLS 

WRITE (NUN ITS. NLINE2A) ( Thpeak_imp ( i ). i-1, 3 )» 

+ ( PT_imp(i). i-i. 3 ). 

+ < ST_imp(i>. i-i, 3 ) 

c *** Calling the Plotting routine to perform plots of the results 
c and store them into the appropiate output files. 

40 CONTINUE 

CALL SCR_PLOTTER 

c *** Generation of output file for the printer: PRINTER.DAT 
REWIND 17 

page_counter - 0 .. 

50 CONTINUE 

page_counter - page_counter + 1 
WRITE( 18. NLINE3) page_counter 

DO 60 i-li 50 ! SO lines/record 

record_counter - record_counter + 1 
IF ( FLAG (2) > 

THEN 

READ<17. *. END-70) FR. TEMPI. TEMP2. 

( Y(j), j-1, 10 ) 

WRITEUB, 950) FR. TEMPI. Y<3>, Y<7>. 

TEMP2, Y(4) » Y(B). 

Y( 1 ). Y(5>. Y<9>, 

Y(2>. Y(6). Y(10) 

CO TO 60 
END IF 

IF ( FLAQ<4> ) 

THEN 



READ <17. •> END 1 

■70) t 

! Reading 

All 

+ 

WRITEUB. 930) 

( Y( j ), j-1. 10 > 
t 

! States 


+ 

( 

Y(j), j-1, 3 ), 

! Writing 

All 

+ 

t 

Y(k>. k-B. 10 ) 

! States 



ELSE 

READ<17. *, END-70) t. ( Y(j). j-1.6 ) 
WRITE( IB. 930) t. ( Y(j). j-1.6 > 


END IF 


60 

CONTINUE 



GO TO 50 

! Return and write new page 

70 

CONTINUE 



RETURN 



C *************************** FORMATS ******************************C 
900 FORMAT ( 1H /1H /1H /1H /1H /1H /1H /1H /1H . SX. 

+ 'In order for you to see the output generated by the'/ 

+ 1H . 5X. 'SIMULATION Program, please. PRESS the NO-SCROLL key'/ 

+ 1H .5*. 'to start looking at the Output...'/ 

+ 1H /1H /1H /1H /1H /1H /1H /1H /1H /1H /1H ) 

910 FORMAT ( 1H /1H . 2X. 'Please. Press the NO-SCROLL key to conti'. 

+ 'nue on next page... ') 

FORMAT ( GB. 3. 6(G12. 4) ) 


920 



930 FORMATdH , G12. 6,/>(G20. 12) ) 

940 FORMAT ( G8. 3. 3< 4G1B. 6/ 8X) ) 

950 FORMATdH , 13G10. 2) 

0================== FORMATS FOR STEP ANALYSIS ====-■»=====*===«=■=—= =»«C. 

1000 FORMATdH . 10X, 'NUMERICAL OUTPUT FOR STEP RESPONSE ANALYSIS 9x. 

+ 'Page '. I3/1H . 

+ lOXf , s=== :; seB3acBC8iec«3aBSBBBn«BaBsaBaaa3KflSBBa ; ^ / 

+ 'Time'.6X. 'OMEGA! x )'. 4X. 'OMECA( g ) ' , 4X, 'DMECA(x)', 

+ 4X< 'THETA(x) 4X. 'THETA(g) '.4X. 'THETA(x) '/'(sec) 

+ 2X.3( 3x. '(rad/sec) ' >.3( 3x. ' (radians) ' ) 

+ /4('-').2X.6(4X,B( '-')>> 

1010 FORMATdH . 20X> 'Characteristics of Step Response ', lOXi 'Page 1' 

+ /1H ,20x,32('-')/lH /1H / 

+ 1H . 10X. '1.- PERCENTAGE OF MAXIMUM OVERSHOOT' * 

+ ' (PMO) '/1H » 14X» 37< ' )/lH / 

+ 1H . 14X, 'PMO (x-axis) - '.012.6.' 7 . '/ 

+ 1H . 14X. ' ( g— axis) - '.012,6.'% •/ 

+ 1H . 14X. ' (x-axis) ■ ',012. 6.' % '/1H / 

+ 1H . 10X. '2. - PEAK TIME (PT) '/1H . 14X. 14( '-')/lH / 

+ 1H . 14X, ' PT ( x-axis) - '. 012. 6. ' sec. '/ 

+ 1H . 14X. ' (g-axis) = ',012.6,' sec.'/ 

+ 1H . 14X. ' (x-axis) - '.012. 6. ' sec. '/1H /1H / 

4- 1H . 2X, 'Please. Press the NO-SCROLL keg to continue ' 

+ 'on next page. . . '/1H /1H ) 

1020 FORMATdH . 20X. 'Characteristics of Step Response'. 10X. 'Page 2' 

+ /1H . 20x. 32( '=' )/lH /1H / 

+ 1H . 10X. '3. - RISING TIME (RT) '/1H . 14X. 16( '-')/lH / 

+ 1H . 14X. ' RT (x-axis) = ',012.6.' sec.'/ 

+ 1H . 14X, ' (g-axis) - ',012.6,' sec.'/ 

+ 1H.14X. ' (x-axis) « '.012. 6. ' sec. '/1H / 

+ 1H , 10X. '4. - DELAY TIME (DT)'/1H , 14X. 15( '-' )/lH / 

+ 1H . 14X, ' DT (x-axis) « '.012,6.' sec.'/ 

♦ 1H.14X. ' (g-axis) - ',012.6.' sec.'/ 

+ 1H . 14X. ' (x-axis) - '.012.6,' sec. '/1H /1H / 

+ 1H . 2X. 'Please. Press the NO-SCROLL keg to continue ' 

'on next page. . . '/1H /1H ) 

1030 FORMATdH . 20X. 'Characteristics of Step Response '. 10X. 'Page 3' 


+ 

/1H 

, 20x. 32 ( 

'-')/lH /1H 

/ 

♦ 

1H . 

10X. '5. - 

SETTLING 

TIME (ST) '/1H , 14X 


1H . 

14X, ' ST 

( x-axis) 

m 

'.012. 6 , ' sec. '/ 

+ 

1H . 

14X, ' 

(g-axis) 

m 

'.012. 6 . ' sec. '/ 

4 - 

1H . 

14X. ' 

( x-axis) 

m 

',012. 6 , ' sec. ') 


1040 FORMAT! 1H1.25X. 'NUMERICAL OUTPUT FOR STEP RESPONSE ANALYSIS', 5x. 

+ 'Page '. I3/1H . 

+ 25X» ' B28S»BBSKBBaBMUBMBaBRM«BnBMBBaB«BCaa ' / 1H0/ 

+ 1H > 2X. 'Time ', 12X. ' OMEGA ( x ) '. 12X. 'OMEGA! g ) '. 12X, 'OMEGA ( ', 

+ 'i) '. 12X, 'THETA(x) ', 12X, 'THETA(g) ', 12X. 'THETA! x) ' 

+ /1H , 2X. ' (sec ) ', 3( 11X. ' (rad/sec > ' >,3( 11X. '(radians) ' ) 

+ /1H »2X.S('-'),6(11X,9('-')) 

+ /1H ) 

FORMATS FOR FREQUENCY ANALYSIS =>,==*======«>=n======aC 

2000 FORMATdH . 10X. 'NUMERICAL OUTPUT FOR FREQUENCY RESPONSE ANALYSIS' 
♦ , 5x, 'Page '. I3/1H . 

+ /1H /1H / 

'Freq. '.6X. 'THETA AMP ( x )', 6X, 'THETA PHA(x)'. 

6X. 'OMEGA AMP! x ) ', 6X. 'OMEGA PHA(x)'/ 

' 6X, 'THETA AMP ( g ) ', 6X. 'THETA PHA(g) ', 


+ 

+ 

+ 


* 




• ► 


'i 


, -j 6 X. 'OMEGA_AMP(y) 6 X. 'OMEGA_PHA(y ) '/ 

' 6 X 1 *THETA_AMP(x) '.&X. 'THETA_PHA( i ) 

6 X. 'OMEGA_AMP( x> '. &X. 'OMEGA PHA(x)' 

/1H ) 

FORMAT ( 1H1. 33X» 'NUMERICAL OUTPUT FOR FREQUENCY RESPONSE ANALYSIS' 
i 5«. 'Page '. I3/1H > 

/1HO/1H . 13( ' + '). ' + '/lH . 

'’Frequency! '. 21X. 'ANGULAR POSITION'. 22X. 'I'.23X. 

'ANGULAR RATE '. 24X. 'I '/1H .'l'.9X. 'i' 

. 9X. 'Amplitude'. 11X. 'I '. 11X. 'Phase'. 13X. 'I '. 9X, 
'Amplitude'. UX. 'I '. UX. 'Phase'. 13X. 'I ' 

/1H . ' I '. 9X, ' I '. 9X. '(Decibels) '. 10X. ' I '. 9X. '(Degrees) '. 

1 IX. ' 1 '. 9X, '(Decibels) '. 10X, ' I ', 9X, '(Degrees) '. UX. 'I ' 

/1H . ' I '. 7X. 4(2X, ' I X-axis'. 4X. 'Y-axis'. 4X. 'Z-axis'). 

' I '/1H . 13( '+ '). '+' 

) 

====»==«===«■ FORMATS FOR IMPULSE ANALYSIS — — •«»»»»• »»C 
FORMAT ( 1H . 10X. 'NUMERICAL OUTPUT FOR IMPULSE RESPONSE ANALYSIS' 

, 5X< 'Page '. I3/1H . 

10X, ' 

/1H/1H . 'Time ', 6 X. 'OMECA( x ) '. 4X. 'OMEGA(y ) '. 4X. 'OMEGA(x)'. 
4X, 'THETA ( x ) '. 4X, 'THETA(y ) '. 4X» 'THETA( x ) '/1H . 4( '-'). 
2X,6(4X.B( '-') ) 


FORMAT ( 1H ,20X. 'Characteristics of Impulse Response'/IH 


4- 

35( '■' )/lHO> 

10X. 'ANGULAR POSITION'/IH 

. 10X. 16( '-')/ 

+ 

1HO. 10X. '1. - 

Peak value 

(x-axis) 

m 

'. G12. 6. ' 

(rad) '/ 

4 - 

1H . 10X. ' 


(y-axis) 

m 

', G12. 6. ' 

(rad)'/ 

4- 

1H . 10X. ' 


(x-axis) 

m 

'.012. 6, ' 

(rad) '/ 

4- 

1HO. 10X. '2. - 

Peak time 

( x-axis) 

m 

'.012. 6, ' 

(sec) '/ 

4- 

1H , 10X, ' 


(y-axis) 

« 

'• G12. b, ' 

(sec ) '/ 

4- 

1H . 10X. ' 


(x-axis) 


'.012. 6. ' 

(sec) '/ 

4- 

1HO. lOx. '3. - 

Settling time 

(x-axis) 

MS 

'. G12. 6. ' 

(sec > '/ 

4 - 

1H . 10X. ' 


(y-axis) 

MS 

'.012. 6, ' 

(sec) '/ 

4* 

1H , 10X. ' 


(x-axis) 


'.012. b, ' 

(sec) ' 


FORMAT ( 1H1. 25X, 'NUMERICAL OUTPUT FOR IMPULSE RESPONSE ANALYSIS' 

. 5x. 'Page '. I3/1H . 

25X. 'Bas=EaiISUEIiaK33HSCaBS3B3»aBZE8aia>UEUBEDa ' 

/1HO/ 

1H • 2X. 'Time'. I2X. 'OMEGA(x) '. 12X. 'OMEGA(y) '. 12X. ' OMEGA ( '. 

'x> '. 12X. 'THETA(x) '. 12X. 'THETA(y ) '. 12X. 'THETA(x) ' 
/1H . 2X> ' (sec) '. 3( 11X. ' (rad/sec ) ' ).3( UX. '(radians) ' > 
/1H . 2X.5('-').6(UX.9('— ')) 

/1H ) 


END 


a> 


♦ 

« 

• 

1 

i 

( 

t 
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( 

V 
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i 
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I 

I 
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c ************************************************************************ 
c * * 

c * Subroutine RUNGE_KUTTA * 

c * * 

c *»*»#**#***«****«#•«»***••****•#•****«*#****#«*#**««****•«««***«*«#**«* 

c *** Solution of a system of first order ordinary differential 
c equations uith given Initial values. 

c Particular situation: 6 variables devided into tuo sets,v one for 

c position and the other for angular velocity. 

c »#* If Quaternion's Computations are going to be performed a nee set 
c of 4 variables is added. 


c **# 
c 


The method applied is by means of 4th order Runge-Kutta formul- 
ation by using the modification due to Gill. 

Written by Juan J. Rodriguez-Moscoso on 22-Jan-85 
Last revised on 01-Jun-B5 


SUBROUTINE RUNGE KUTTA ( ndim 

to 

tf 

tincr 

EPS 

Y 

Ydot 


! Dimension of States. 

! Initial time. 

! Final time. 

! Time increment. 

! Precision Epsilon. 

! Array of States. 

! Derivative of States. 
) ! Error Return code. 


COMMON AREAS DEFINITIONS AND ARRAYS 


IMPLICIT DOUBLE PRECISION (A-H.0-2) 


DIMENSION 


Y(10> 

a<4> 


. Ydot ( 10) 
. b < 4 ) 


auz ( 16, 10) 
c (4) 


COMPUTATIONS 


DO i«*l«ndim 

au»(B, i) - 0. 066666666666666667d0*Ydot< i) 
END DO 


t “ to 

tend «■ tf 
deltat ■ tincr 
I CODE * 0 


Starting time. 
Ending time. 
Time increment. 
Stop condition. 


CALL SYS_DYNAM < ndim, t, Y, Ydot > 

IF ( del tat*( tend-t > > 3BO, 370, 20 ! Error Test. 


i 


20 


a ( 1) =0. 5d0 


a( 2) = 0, 292893?1881345248d0 
a ( 3) = 1. 707106781 1865475d0 
a ( 4) = 0. 1666 6666666666667 d 0 


b ( 1) = 2. OdO 
b ( 2) = 1. OdO 
b < 3) = 1. OdO 
b ( 4) = 2. OdO 

c< 1) - 0. 5d0 
c< 2 ) * a ( 2 ) 
c< 3) = a ( 3) 
cC 4) =0. 5d0 

DO 1=1, ndim ! First Runge-Kutta step, 

aux ( 1, i> « Yt i > 
aux ( 2. i) « Ydot(i) 

aux <3. i > - O. dO 
aux ( 6. i ) x 0. dO 
END DO 

irec “ 0 

deltat x deltat + deltat 
ihlf x -1 
istep x o 
iend = o 

40 IF ( (t + deltat - tend)*deltat ) 70, 60. 50 

SO deltat x tend — t 

60 iend x \ 

70 CONTINUE 

CALL 0UTPUT_17 ( ndim, t. tincr. Y. Ydot ) 

CALL CHECK ERROR ( irec, ICODE > 

IF { ICODE . NE. 0 ) RETURN 1 
itest x 0 

90 istep “ istep + 1 

J x 1 • Start of innermost Runge-Kutta loop. 

100 aj x a ( j) 

b j x b ( j ) 
cj - c< j) 

DO ixl.ndim 

rl x del tat*Ydot ( i > 

r2 x aj*< rl-b j«aux (6, i > ) 

Y(i> - Y< i ) + r2 
r2 o r2 + r2 + r2 
aux (6, 1) x aux(6, i) + r2 - cj*rl 
END DO 



IF 

( 

j - 4 ) 

120. 

150. 

120 

J * 

J 

+ 1 




IF 

( 

J - 3 > 

130. 

140. 

130 

t x 

t 

+ 0. 5d0*del tat 

140 

CALL 

SYS DYNAM ( 

ndim. 


GO 

TO 

100 



150 

IF 

( 

itest ) 

160. 

160. 

160 

DO 

i = 

1, nd im 




aux ( 4. i) = YCi) 
END DO 


150 

130 

t, Y, Ydot ) 

200 ! Test of Accurracy 


I 


1 


t 



180 



itest = ^ 

istep = istep + istep - 2 

ihlf = ihlf + 1 
t = t - deltat 
de l tat = 0. 5d0*del tat 
DO i°l,ndim 

Y(i) ° aui( 1. i) 
Ydot(i) = aui ( 2, i) 
aux( 6. i ) = aux( 3. i) 
END DO 
GO TO 90 


200 imod = istep/2 

IF ( istep - imod - imod ) 210. 230. 210 
210 CONTINUE 

CALL SYS_DYNAM < ndim. t. Y. Ydot ) 

DO i»l, ndim 

aux (5. i > = Y ( i ) 
aux (7. i ) *■ Ydot ( i ) 

END DO 
GO TO 90 




J 


J 


» 


I 


J 


>> 


j 


J 

0 


230 delt *■ 0. ODO 


DO 

i=*l. ndim 






delt “ d 

elt + aux<8, 

i )»DABS( 

ENC 

1 DO 





IF 

< delt - 

EPS 

) 280, 

2B0. 

250 

IF 

< ihlf - 

10 

) 260. 

360, 

360 

DO 

i*l. ndim 






aux< 4, 

i) - 

aux( 5. 

i) 



END DO 

istep = istep + istep - 4 
t *• t - deltat 
iend = 0 
CO TO 180 


c ••• Result values are good 
280 CONTINUE 

CALL SYS_DYNAM t ndim, t, Y. Ydot ) 


DO i«l 

< 

ndim 



aux 

c 

1. 

i) 

*■ Y(i) 

aux 

{ 

2. 

i) 

“ Ydot( i ) 

aux 

< 

3. 

i) 

= aux( 6, i) 



Y(i) 

n 

aux( 5. i) 


Ydot(i) “ aux( 7. i) 

END DO 

CALL 0UTPUT_17 ( ndim. t-deltat. tincr, Y. Ydot 1 
CALL CHECK_ERROR ( ihlf, I CODE > 

IF ( I CODE .NE. 0 ) RETURN 1 
DO i«>l,ndim 

Y< i ) «= aux ( 1. i) 

Ydot ( i ) ° aux ( 2. i) 

END DO 
irec = ihlf 

IF ( iend > 320. 320. 390 


* 320 ihlf “ ihlf - 1 ! Increment gets doubled, 

istep = istep/2 
deltat = deltat + deltat 

-0 


I 

< 


¥ 



330 . 

340 

350 

360 

370 

380 

390 


IF ( ihlf, ) 40. ^30. 330 
imod = istep/2 

IF ( istep - imod - imod ) 40. 340. 40 

IF ( d el t - 0. 02d0*EPS ) 350. 350. 40 
ihlf = ihlf - 1 
istep = istep/2 
deltat =* deltat + deltat 
GO TO 40 

ihlf «= 11 

CALL SYS_DYNAM ( ndim, t. Y. Ydot ) 

GO TO 390 

ihlf - 12 
GO TO 390 

ihlf = 13 

CALL 0UTPUT_17 ( ndim, t. tincr. Y. Ydot ) 
CALL CHECK ERROR ( ihlf. ICODE > 

IF < ICODE . NE. 0 ) RETURN 1 

RETURN 

END 



c ******«**»»»**»f *«#»**i‘***»««*»#*******»***»*«*»*»»****»**»»««*»«'*»*«** 
c * * 
c * Subroutine SCR_PLOTTER * 
c * • 
c *####**#****#**»*#********#*#****###**#**##****#*####***##***#*#**#**** 


c Written bg Juan J. Rodr iguei-Moscoso on 07-May-B5 

c This routine reads in a file F0R017.DAT previouslg created with 

c all data to be plotted. Then. it proceeds to build a screen plot 

c of this data for VTlOO's like terminals. » 

SUBROUTINE SCR PLOTTER 


COMMON AREAS AND ARRAYS DEFINITIONS 


IMPLICIT DOUBLE PRECISION (A-H. O-Z) 


c *** Common Areas Definitions: 
COMMON /READIN/ Bound (15) 
+ Kd (3. 3) 

+ Thcom(3) 


Yin(lO) 
INMAT (3, 3) 
Omcom(3) 


Kp (3. 3) 

I NMATV ( 3i 3) 


COMMON / FLAG / FLAG 

COMMON / 8CRP / number_records, ! Step Response 

number_decadesi ! Frequency Response 
number_samp_f 


c *** Dimensioning of Arrays 

DIMENSION Y(12. 61). 

Ymax ( 12), 
Ymin( 12)i 
t (7) 


! Plot of axes 


c *** Variable definitions 

LOGICAL*l FLAG (7) 

REAL*B Kp , Kd . INMAT . I NMATV 

CHARACTERS p lot_symbol <6) , blank, bode.symbol (4) 

CHARACTERS home_pos 

CHARACTER *4 upl. TITUL02A 

CHARACTER*5 mover, moved. curs_pos. moverlO. TITUL07B. 

TITULOB 

CHARACTER *7 TITUL03A, TITUL03B 

CHARACTER*B TITUL02B 

CHARACTER*9 TITUL07A, UNIDADO) 

CHARACTERelO bianco 

CHARACTER#12 TITUL04A. TITUL04B. TITUL06. TITUL09 

CHARACTER*! 5 TITULOl 

CHARACTER*17 TITUL05 

CHARACTER**.*. TITULO 


plot_symbol/'x'. 'y '. 'x'. 'x'. 'y '. 'z '/. blank/' '/ 
TITULOl/ 'Plot of Angular'/. 

TITUL02A/ 'RATE '/. TITUL02B/ 'POSITION '/. 

TITUL03A/ ' [Omega ( '/, TITUL03B/ ' CTheta ( '/. 

TITUL04A/ ' ) 3 (rad/sec)'/. TITUL04B/ ' ) 3 (radians)'/ 
TITUL05/'vs Time (seconds)'/. 

TITUL06/ 'Bode Plot of'/. 

TITUL07A/ 'AMPLITUDE'/, TITUL07B/ 'PHASE '/. 


DATA 

DATA 

DATA 


T,ITULOB/ '-axis'/. 

TITUL09/ 'vs FREQUENCY'/ 

UNIDAD/Mdecibel) '. '(degree*)', '(rad/sec)'/ 
TITULO/' '/. bianco/' '/ 
bode_symbol/'A'. 'P'. 'A'. 'P'/ 


c* PLOTTING * 


c *** Check out FLAGs to determine how many plotting files are needed. 
REWIND 17 

call cursor_right( moverlO. 1. idumm. 10 ) 
call cursor_up< upl. 1. idumm. 1 ) 
call cursor_home( home_pos< 1. i dummy ) 

DO i«l,3 

IF < FLAG ( i ) ) 

THEN 

GO TO < 1. 2. 3 ). i 
END IF 
END DO 


cucEsmosetssKu.u, THETAOPLOT. STP & 0MECA0PL0T. STP 
1 CONTINUE 

i*_salto >* <number_reeords - l)/60 
i*_re*to “ MOD< number_record* - 1, 60 ) 
i*_resal - i*_salto 
kt ■ 1 

IF < FLAG (4) ) 

THEN 

READ! 17. *) t(l>. ( Y(k.l), k»1.3 ). TEMP. TEMP. TEMP. TEMP. 

( Y(k. 1>» k=4. 6 > 

DO I - 1.60 

DO J > t. is_retal 

READ! 17. *) t ( kt+1 ) . ( Y(k.I + l). k-1.3 >. TEMP. TEMP. 
TEMP. TEMP. C Y(k.I+l>. k-4. 6 > 

END DO 

IF ( MOD(I.IO) . EQ. 0 ) kt - kt + 1 
END DO 
ELSE 

READ( 17. *) t(l>. ( Y(k.l), k-1.6 ) 

DO I = 1.60 

DO J ■ 1. is_re»al 

READ< 17, * ) t < kt+1 ). ( Y(k. 1+1). k-1.6 > 

END DO 

IF ( MOD(I.IO) .EQ. 0 ) kt » kt + 1 
END DO 
END IF 

DO I - 1.6 

Ymax(I) - 0. 0 
Ymin(I> *• 0.0 
END DO 
DO I = 1,60 
DO J> 1.6 

IF < Y(J. I) .OT. Ymax < J) > Ymax(J) ■ Y(J. I) 

IF ( Y(J. I) LT. Ymin(J) > Ymin(J> - Y(J. I> 

END DO 



iunit =9 , 

DO I = 1* 6 ! Plotting first OMEGAOPLT. STP for every axis. 

! and then THETAOPLT. STP 

TITULO = ' ' 

TITULO! 1: 2B) = TITUL01//b lank//TITUL02A//blank//TITUL03A 
TITULO (29: 59) = plot symbol! i ) //TITUL04A//b lank //TITUL05 
IF ! I . GT. 3 ) 

THEN 

iunit = B 

TITULOt 17: 63) » TITUL02B//blank//TITUL03B//plot symbol(i) 
//TITULD4B//b lank//TITUL05. 

END IF 

call c lear_disp lay < iunit.'*' ) 

call drau_aies! iunit. Ymai(I). Ymin(l). t. kt ) 

WRITE< iunit. *> home_pos. bianco. TITULO 
call cursor_douin! moved. 1. idummy. 21 ) 
i dummy «= idummy - 1 

WRITE! i uni t • * ) moved ( 1 : idummy ) ■ upl 

posit » 20. 0*( Yd. 1 » - Ymin(I) )/( Yma» ( i )-Ymin( I > > + i. 0D0 

neu_position = NINT(posit) 

call cursor_up( moved. 1. idumm. new_position ) 
idumm = idumm-1 

WRITE! iunit. *) moved ( 1: idumm). moverlO. plot_symbol(i). upl 
old_pos ■= posit 
DO J-l, 60 

posit = 20. 0*( Y! i. j+1 )-Ymin! i ) )/!Ymax!i)-Ymin!i))+l. 0 
is_posic ■« NINT(posit) - NINT( old_pos ) 
old_pos “ posit 
ne\ii_position » is_posic 
IF ( neui_position . GE. 0 ) 

THEN 


call curtor_up( curs_pos> 1. idum. neui_position ) 
ELSE 

call cursor _douml curs _pos, 1. idum. 

IABSineu_position) > 

END IF 

idum <• idum-1 

call cursor_right< mover. 1. idumm. J ) 

idumm = idumm - 1 

IF ( neu_position . EQ. 0 > 

THEN 

WRITE! iunit. *) moverlO. mover ! 1: idumm). 

plot symbolli). upl 

ELSE 

WRITE! iunit. *) curs_pos! 1: idum). moverlO. 

mover! 1: idumm). plot_symbol!i). upl 


END IF 
END DO 


WRITE! iunit. *) home_pos 

call cursar_douni moved. 1. idummy. 21 ) 

idummy=i dummy - 1 

WRITE! iunit. *) moved ! 1: idummy ) 


END DO 
RETURN 


CONTINUE 


THETAOAMP. FRO it THETAOPHA. FRO 
OMEGAOAMP. FRO it OMEGAOPHA. FRO 


c 



k t = 0 

number.rectords = number_decades*number_samp_f + 1 
DO i=l» number records 

IF < Bound ( 13) . EQ. 1. 0D0 > 

THEN 

READ(17,*) t(kt+l). ( Y(k. i). k-1.4 ). (TEMP. k=1.8> 
iaxis b i 
ELSE 

IF ( Bound (14) . EQ. 1. ODO > 

THEN 

READ( 17. *) t(kt+l ). (TEMP. kBi,4>. ( Y(k.i). k-1, 4 
. ( TEMP, k-1.4 > 

iaxis b 2 
ELSE 

READ( 17. *> t(kt+l>. (TEMP, k-1.8). ( Y(k.i). k-1.4 
iaxis - 3 
END IF 
END IF 

IF ( MOD( i-1. number samp f ) . EQ. 0 ) kt - kt + 1 
END DO 
DO i-1. 4 

Ymax(i) - 0. ODO 
Ymin( i ) - 0. ODO 
END DO 

DO i-1. number records 
DO j-1,4 

IF ( Y(j.i) . CT. Ymax(j) ) Ymax(j) - Y(j.i) 

IF ( Y(j.i) . LT. Ymin(j) ) Ymin(j) - Y(j.i) 

END DO 
END DO 

DO 1-1.4 

TITULO - ' ' 

TITULO( 1:13) - TITULQ6//b lank 
TITULO( 14: 22) = TITUL02B//b lank 

IF ( I .OT. 2 ) TITULO! 14: 22) b ' '//TITUL02A// ' ' 

IF ( MOD( 1-1,2) .EQ. 0 ) 

THEN 

TITUL0(23: 36) - TITUL07A // ' for ' 

TITUL0(37: 55) - plot.symbol ( iax is) //TITULOB/Zblank// 
TITUL09 

bianco - UNIDAD(l) 

ELSE 

TITUL0(23: 32) - TITUL07B// ' for ' 

TITUL0(33: 51) - p lot_symbol ( iaxis )//TITUL08//b lank// 
TITUL09 

bianco - UNIDAD(2) 

END IF 

iunit -9+1 

call clear .display ( iunit. ) 

call drau.axes ( iunit, Ymax(I). Ymin(I). t. kt ) 

WRITE! iunit, #) home_pos. bianco. ' TITULO! 1:55) 

bianco - ' ' 

call cursor_doun( moved. 1. idummy. 20 ) 
call cursor.r ight( mover, 1. id urn. 71 ) 
idummy - idummy - 1 
idum - iduin - 1 

WRITE! iuni t. » ) moved ( 1 : idummy ). mover ( 1 : idumm) . UNIDAD(3), 
home jos 

call cursor_doun( moved, 1, idummy. 20 ) 


i 


< 

€ 

< 

) 

I 

) 



U n 


i dummy = i dummy — 1 
WRITE! i uni t. *)* moved ( 1 : i dummy ) 

posit - 20. 0*( Y(I.l) - Ymin(I) )/( Ymax ( I )-Ymin(I > >+l. OdO 

new_position = NINTiposit) 

call cursor_up( moved> 1> idumm> neui_posi tion > 
idumm = idumm-1 

WR1TE( iunit. *) moved ( 1 : idumm) . moverlO. bode_symbol ( I ) . upl 
old_pos = posit 

DO J=l. number_records-l 

posit =* 20.0*1 Y! i. j+1 )-Ymin! i ) )/( Ymax < i )-Ymin( i ) )+l. 0 
is_posic = NINTiposit) — NlNT(old_pos) 
old_pos ** posit 
neu_position = is_posic 
IF < neui_position . CE. 0 > 

THEN 

call cursor up( curs_pos> 1. idum. new .position ) 

ELSE 

call cursor_down( cur*_pos. 1. idum. 

IABS!neu_position) ) 

END IF 

idum = idum - 1 

K = &0*J/1 number .records - 1> 
call cursor.right! mover. 1. idumm. K ) 
idumm ■ idumm — 1 
IF ( new_position . EQ. 0 ) 

THEN 

URITE(iunit) *) moverlO. mover! 1: idumm). 

bode symbol(i). upl 

ELSE 

WRITE! iunit. *) curs _pos( 1: idum). moverlO. 

mover < 1: idumm). bode.symbol ( i ). upl 

END IF 
END DO 

WRITE! iunit. *) home_pos 

call cursor.down! moved. 1. i dummy. 21 ) 
i dummy = i dummy - 1 
WRITE! iunit. *) moved ( 1 : idummy ) 

END DO 

RETURN 

THETAOPLT. IMP It OMEGAOPLT. IMP c 

CONTINUE 

c Non implementation for IMPULSE RESPONSE Plotting. 

RETURN 

END 


< 

! 

I 

( 


i 



c I Copyright*^) NASA Marshall Space Flight Center I 
c I Hunstville. Alabama I 
c i 1985 I 


c a********************************************************************** 
c * * 
c * Subroutine SIMULATION * 
c * * 
c a********************************************************************** 


c *** Written by Bor-Jau Hsieh (Andy) & 

c Juan J. Rodriguaz-Moscoso on 03-Jun-B5 

SUBROUTINE SIMULATION ( ICODE ) 



c* COMMON AREAS AND DIMENSION ARRAYS * 

IMPLICIT DOUBLE PRECISION (A-H.O-Z) 



COMMON 

/ 

FLAG 

/ 

FLAG (7) 






COMMON 

/ 

USER 

/ 

user_choice 






COMMON 

/READIN/ 

Bound ( 15) 

0 

YinUO) 

9 

Kp<3.3> 

+ 





Kd<3, 3) 

9 

INMATO. 3) 

0 

INMATVO. 3) 

+ 





Thcom(3) 

0 

0mcom(3) 




COMMON 

/ 

STEP 

/ 

PM0(3> 

9 

RTini (3) 

9 

RTend (3) 

4- 





DT(3) 

9 

ST(3) 

9 

PT(3) 

+ 





RT(3) 

9 

RTimin(3) 

9 

RTemin (3) 

+ 





DTminO) 

9 

Thpeak (3) 




COMMON 

/ 

FRQ1 

/ 

Amp 

9 

Omega 

9 

Phase 

+ 





ts 






COMMDN 

/ 

FRG2 

/ 

THAMPO) 

9 

THFEE(3) 

9 

THAOLO) 

+ 





THP0L13) 

9 

THS ( 3 ) 

9 

THC<3) 

+ 





XI (3) 

9 

X2C3) 

9 


+ 





OMAMP ( 3 ) 

9 

0MFEE(3) 

9 

0MA0H3) 

+ 





0MP0LC3) 

9 

OMS < 3 ) 

9 

0MC(3) 

+ 





X3(3) 

0 

X4 (3) 

9 

XN 


COMMON 

/ 

FRQ3 

/ 

Freq 

0 

Const 

9 

Tau 


COMMON 

/ 

IMPU 

/ 

ST_imp (3) 

9 

PT_imp (3) 

9 

Thpeak_imp (3) 


COMMON 

/ 

DAMP 

/ 

damp_f lag 






DIMENSION 


Y( 10) 

9 

Ydot(lO) 




INTEGER user_choice» damp flag 

REAL*B Kp , Kd . “iNMAT . INMATV 

LOG I CAL* 1 FLAG 

CHARACTER*? Response_type 

DATA Response_type/' '/ 


c* COMPUTATIONS 

c 

c ••• Initializing of the common input parameters. 

ndim ■ 6 ! dimension of the system 

to » Bound(l) ! initial time 

tf «= Bound(2) ! final time 

deltat «= Bound(3> ! time increment 




c *** Check if damping exits. 
damp_flag ^ 0 ** 

DO 1=1,3 
DO J=l, 3 

IF ( Kd(I.J) . NE. 0. ODO > 

+ THEN 

damp flag = 1 
GOTO - 10 
END IF 
END DO 
END DO 

10 CONTINUE 

c *** Computing the inverse of the Inertia Matrix. 

DO 1 = 1,3 
DO J-1,3 

INMATVU.J) = INMATU.J) 

END DO 
END DO 

CALL MATINV< INMATV, 3. ICODE ) 

IF ( ICODE .NE. 0 ) RETURN 

c *#* Checking FLAG(i) Ci=l,33 for determining type of analysis 
IF ( FLAG(l) ) THEN 
c *** STEP Response Analysis 


1 CONTINUE 

c — > Setting Input Commands to Step. ! May-28-85 

DO j=l. 3 

Thcom(j) = Bound ( j+12) 

Omcom(j) = Bound (j+9) 

END DO 

c — ■> Initialization for step response characteristics analysis 

DO j=l. 3 

RTimin(j) = Thcom(j) 

RTemin(j) = Thcom(j) 

DTmin(j) = Thcom(j) 

PMO( j) = 0. ODO 
RT( j) = 0. ODO 
DT( j) = 0. ODO 
ST( j ) = 0. ODO 

END DO 
Nrt = 1 

Response type = 'STEP' 

GOTO 100 
END IF 

c see Frequency Response Analysis 


IF ( FLAG(2) > THEN 
c — > Initialize input parameters 

DO j = l « 3 

Thcom(j) = 0. OdO ! Input Commands = 0.0 for 

Omcom(j) = 0. OdO ! Pseudo Open-Loop Frequency Response 

END DO 

Tau = Dound(P) ! Time constant 

Amp = Bound (B) ! Amplitude 




! Phase 

! # of decades 

! ft of sampling freq. /decade 




c *** 
c 


C SBBBI 

too 

c 

c 


110 

120 

1000 

c 1010 
c 
c 
c 


Phase “ Bound (10) 

Ndec = Bound(ll) \ 

Nsd = Bound(12) 
deltat = Bound(3) 

Const = DEXP ( (DL0G( 10. 0D0) ) /DFLOAKNsd ) 
Freq => Bound (9) /Const ! Lowest angular 
Nsf » NdeceNsd + 1 
Nrt “ Nsf 

Response_type = 'FREQUENCY' 

GOTO 100 
END IF 


) 

Freq 


IMPULSE Response Analysis 


IF ( FLAG (3 ) ) THEN 
Nrt - 2 

Response_type 'IMPULSE' 
END IF 


STARTINO SIMULATION ———————— 

CONTINUE 

URITE(6> 1000) Response type 
WRITE (6, 1010) 

READ(5i*) user_choice 

user choice " 1 ! This will be taken out. 

DO i-1. Nrt 

CALL INITIALIZE ( Nrt 

ndim * 

to 

tf 

deltat • 

Y ) 

CALL INTEGRATION (ndim 

to . 

tf 

deltat > 

Y 

Ydot . 

LI 10 , 

S.120 ) 

CALL POLFRCAL < deltat ) 

END DO 

CALL RESULTS ( Y ) 

RETURN 

ICODE » 2 
RETURN 
ICODE - 3 
RETURN 

FORMAT (1H0. 5X. 

55HF0RTR AN-77 'Generic Simulation Program' under Execution 
/1H . 5X, 55( '-' )/lHO. 5X. ' - Type of response analysis se'» 
'lected * '.A ) 

FORMAT! 1H . 5X. ' - Would you like to see partial results during '< 
'the execution '/1H • 9X> 'of the simulation program?'/lH > 
20Xi ' 1 ) yes'/lH . 20X. '2) no'/lH . 9X. 'Select your choice:' 
/1H . 12X. '« '*> 

END 


*■ 




t 


t 


% 


V 


s 

V 





c ^ ^ 

c I Copyright (c) NASA Marshall Space Flight Center I 
c ! Hunstville. Alabama t 
c t 1985 I 
c 

c a********************************************************************** 
c * a 
c a SUBROUTINE SUM_ERRORS a 
c a a 


c *********************************************************************** 

c This routine calculates the errors between 

c INPUT COMMANDS and OUTPUTS 

c at summing junction of the system block diagram 

c aaa Written by Juan J. Rodriguez-Moscoso & 
c Bor-Jau Hsieh (Andy) on 04-May-85 

c Last revised on 13-May-S5 

SUBROUTINE SUM ERRORS! ndim. Thcom. Omcom. Y. Therr. Omerr ) 


c# COMMON AREAS AND DIMENSION ARRAYS a 

IMPLICIT DOUBLE PRECISION (A-H. 0-Z) 


c aaa Common Areas Definitions: 
COMMON / FLAG / FLAG ( 7 ) 


c aaa 


+ 

+ 


Dimensioning of Arrays 
DIMENSION Y(10). 

Thcom(3)> 

0mcom(3) 


DIMENSION 

+ 


Therr ( 3) > 
Omerr (3) 


c aaa Variables Definitions 
LOGICAL*! FLAG 


Vehicle Theta & Omega 
Input Command Position 
Input Command Rate 

Position Error 
Rate Error 


c* COMPUTATIONS a 


c aaa If the Quaternion block is considered during the SIMULATION! then 
c the Computation of Errors are performed only for the angular Pos- 

c ition of the vehicle. 

IF ( .NOT. FLAG (4) ) GOTO 100 
DO i » 1.3 

Therr ( i ) - Thcom(i) - Y(i+7) ! Ang. pos. from Y(8> to Y(10). 

Omerr(i) ** - Y(i) ! Unvariable Omega v. 

END DO 
RETURN 

c aaa Compute Angular Position & Rate Errors for Prototype I system. 

100 CONTINUE 

DO j = 1.3 

Therr(j> ■ Thcom(j) - 
Omerr(j) ** Omcom! j) - 
END DO 


Y< j-t-3) 
Y( j) 



aN3 

NUD13M 



c 

c 

c 


w V- . 

Copyright (c) NASA Marshall Space Flight Center 
Hunstville> Alabama 
1985 


c *********************************************************************** 
c * * 

c * Subroutine SYS_DYNAM * 

c * * 

c ****#******#****#*#*###**##******#**#*#****####****#*#******»#****#*### 

c *** Written by Juan J. Rodr iguez-Moscoso & 
c Bor-Jau Hsieh (Andy) on 30-Apr-B5 

c Last revised on 2V-May-B3 

SUBROUTINE SYS_DYNAM < ndiro. t. Y. Ydot ) 


COMMON AREAS AND DIMENSION ARRAYS 



IMPLICIT DOUBLE 

PRECISION 

(A-H.O-Z) 




COMMON / FLAG / 

FLAG ( 7 ) 





COMMON /READIN/ 

Bound (15) 

. Yin(lO) 

. Kp (3. 3) 

• 

4- 


Kd (3. 3) 

. INMAT (3, 3) 

, INMATVO. 3) 

» 

+ 


Thcom(3) 

\ 

, 0mcom(3> 




DIMENSION 

Y( 10) 

. Ydot ( 10) 

.Therr(3> 

9 

+ 


Omerr (3)' 

. Torque(3) 




REAL*B Kp , Kd INMAT . INMATV 

LOG I CAL* 1 FLAG 


COMPUTATIONS 


CALL SUM ERRORS ( ndim> Thcomi Omcom. Y> Therr> Omerr ) 
CALL ANALYSIS ( ndim. t. Y. Therr. Omerr ) 

CALL CONTROLLER ( ndim< Therr# Omerr# Torque ) 

CALL BQDY_DYNAM < ndim, Torque. Y. Ydot ) 

CALL QUATERNION ( ndim. Y. Ydot ) 

CALL NEWJMLUES ( ndim. Y ) 

RETURN 

END 


k 

k 

i. 




End of Document 



